def checkCollision(world, robot, q): robot.setConfig(q) collisionChecker = collide.WorldCollider(world) coll = collisionChecker.robotObjectCollisions(world.robot(0)) for i, j in coll: return True return False
def __init__(self,world): vis.GLPluginInterface.__init__(self) self.world = world self.collider = collide.WorldCollider(world) self.quit = False #adds an action to the window's menu def doquit(): self.quit = True self.add_action(doquit,"Quit",'Ctrl+q',"Quit the program")
def Planning(self): self.nodeList = [self.start] robo = self.robot collisionChecker = collide.WorldCollider(self.world) while True: # Random Sampling if random.randint(0, 100) > self.goalSampleRate: rnd = [ random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand) ] else: rnd = [self.end.x, self.end.y] # Find nearest node nind = self.GetNearestListIndex(self.nodeList, rnd) # print(nind) # expand tree nearestNode = self.nodeList[nind] theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) newNode = copy.deepcopy(nearestNode) newNode = VDP2(newNode.x, newNode.y, theta, self.expandDis) newNode.parent = nind robo.setConfig(newNode) collRT2 = collisionChecker.robotObjectCollisions(self.world.robo) for i, j in collRT2: collisionFlag = True continue self.nodeList.append(newNode) # check goal dx = newNode.x - self.end.x dy = newNode.y - self.end.y d = math.sqrt(dx * dx + dy * dy) if d <= self.expandDis: print("Goal!!") break #if animation: #self.DrawGraph(rnd) path = [[self.end.x, self.end.y]] lastIndex = len(self.nodeList) - 1 while self.nodeList[lastIndex].parent is not None: node = self.nodeList[lastIndex] path.append([node.x, node.y]) lastIndex = node.parent path.append([self.start.x, self.start.y]) return path
def registerCollisionFunction(world, name='collisionFree'): collider = collide.WorldCollider(world) robot = world.robot(0) def collides(x): robot.setConfig(x) for c in collider.collisions(): return False return True registerFunctionFactory(name, lambda args: collides)
def gen_rrt(world, robot, q_init, num_samples): # Used to check if a configuration results in collision in the world. coll_check = collide.WorldCollider(world) # First, build tree with initial configuration. rrt = RRT(q_init) # Now, for k iterations, try to expand the tree. for k in range(0, num_samples): q_rand = gen_rand_config(world) rrt.extend_tree(q_rand, world, robot, coll_check) return rrt
def __init__(self, visWorld, planningWorld, name="IK Database visual tester"): GLWidgetProgram.__init__(self, visWorld, name) self.planningWorld = planningWorld self.collider = collide.WorldCollider(visWorld) self.ikdb = ManagedIKDatabase(planningWorld.robot(0)) self.ikWidgets = [] self.ikIndices = [] self.ikProblem = IKProblem() self.ikProblem.setFeasibilityTest('collisionFree', None) qmin, qmax = planningWorld.robot(0).getJointLimits() self.ikProblem.setCostFunction('jointRangeCost_dynamic', [qmin, qmax]) self.drawDb = False self.continuous = False self.reSolve = False self.currentConfig = self.world.robot(0).getConfig()
def WillCollideDuringClosure(hand, obj): world = hand.world robot = hand.robot q_old = robot.getConfig() sigma_old = hand.getCommand() collision = collide.WorldCollider(world) #init list_r_w_coll = [] # same as above. The output is generator so make a list to know how many collision we have been for i in range(6): hand.setCommand([i/5.0]) tau, q = hand.output(kinematic = True) robot.setConfig(q) r_w_coll = collision.robotTerrainCollisions(robot) # check collision robot-plane for coll in r_w_coll: list_r_w_coll.append(coll) hand.setCommand(sigma_old) robot.setConfig(q_old) if len(list_r_w_coll) >0: #if the length is greater that zero, we have collision return True else: return False
def CollisionTestInterpolate(world, robot, obj, w_P_h_goal, w_P_h_start = None): q_old = robot.getConfig() if not isinstance(w_P_h_goal, tuple): w_T_h_goal = se3.from_homogeneous(w_P_h_goal) #o_P_h is end-effector in object frame else: w_T_h_goal = w_P_h_goal if w_P_h_start is None: w_T_h_start = get_moving_base_xform(robot) else: if not isinstance(w_P_h_start, tuple): w_T_h_start = se3.from_homogeneous(w_P_h_start) # o_P_h is end-effector in object frame else: w_T_h_start = w_P_h_start t_des = w_T_h_goal[1] t_curr = w_T_h_start[1] set_moving_base_xform(robot, w_T_h_goal[0], w_T_h_start[1]) step_size = 0.01 d = vectorops.distance(t_curr, t_des) n_steps = int(math.ceil(d / step_size)) if n_steps == 0: # if o_T_h_current == o_T_h_desired return CheckCollision(world, robot, obj) collider = collide.WorldCollider(world) for i in range(n_steps): t_int = vectorops.interpolate(t_curr,t_des,float(i+1)/n_steps) set_moving_base_xform(robot, w_T_h_goal[0], t_int) if CheckCollision(world, robot, obj, collider): robot.setConfig(q_old) return True robot.setConfig(q_old) return False
def CheckCollision(world, robot, obj, collision = None): """ :param world: world object :param robot: robot object :param obj: object to check collision against :collision: an optional WorldCollider :return: True if in collision """ if collision is None: collision = collide.WorldCollider(world) #init r_o_coll = collision.robotObjectCollisions(robot,obj) #check collision robot-object. the output is generator list_r_o_coll = [] # make a list to know how many collisions we have been for coll in r_o_coll: list_r_o_coll.append(coll) r_w_coll = collision.robotTerrainCollisions(robot) # check collision robot-plane list_r_w_coll = [] # same as above. The output is generator so make a list to know how many collision we have been for coll in r_w_coll: list_r_w_coll.append(coll) if len(list_r_o_coll) > 0 or len(list_r_w_coll) >0: #if the length is greater that zero, we have collision return True else: return False
robot.setAltitude(0.12) #robot = sphero6DoF(world.robot(0), "sphero", vis) ## Display the world coordinate system vis.add("WCS", [so3.identity(), [0, 0, 0]]) vis.setAttribute("WCS", "size", 24) #print "Visualization items:" #vis.listItems(indent=2) #vis.autoFitCamera() vis.addText("textCol", "No collision") vis.setAttribute("textCol", "size", 24) collisionFlag = False collisionChecker = collide.WorldCollider(world) ## On-screen text display vis.addText("textConfig", "Robot configuration: ") vis.setAttribute("textConfig", "size", 24) vis.addText("textbottom", "WCS: X-axis Red, Y-axis Green, Z-axis Blue", (20, -30)) print "Starting visualization window#..." ## Run the visualizer, which runs in a separate thread vis.setWindowTitle("Visualization for kinematic simulation") print("Starting.....") source = [0, 0, 0, 0, 0, 0] goal = [3, -3.5, 0, 0, 0, 0]
def create(self, start_pc, goal_pc): """ This method cretes the simulation :param start_pc: robot's initial position coordinate :param goal_pc: goal position coordinate :return: """ print "Creating the Simulator" object_dir = "/home/jeet/PycharmProjects/DeepQMotionPlanning/" self.start_pc = start_pc self.goal_pc = goal_pc coordinates.setWorldModel(self.world) getDoubleRoomDoor(self.world, 8, 8, 1) builder = Builder(object_dir) # Create a goal cube n_objects = 1 width = 0.1 depth = 0.1 height = 0.1 x = goal_pc[0] y = goal_pc[1] z = goal_pc[2] / 2 thickness = 0.005 color = (0.2, 0.6, 0.3, 1.0) builder.make_objects(self.world, n_objects, "goal", width, depth, height, thickness, self.terrain_limit, color, self.goal_pc) # Create a obstacle cube n_objects = 4 width = 0.2 depth = 0.2 height = 0.2 x = 2.3 y = 0.8 z = goal_pc[2] / 2 thickness = 0.001 color = (0.8, 0.2, 0.2, 1.0) builder.make_objects(self.world, n_objects, "rigid", width, depth, height, thickness, self.terrain_limit, color) self.vis = vis vis.add("world", self.world) # Create the view port vp = vis.getViewport() vp.w, vp.h = 1200, 900 vp.x, vp.y = 0, 0 vis.setViewport(vp) # Create the robot self.robot = sphero6DoF(self.world.robot(0), "", None) self.robot.setConfig(start_pc) # Create the axis representation # vis.add("WCS", [so3.identity(), [0, 0, 0]]) # vis.setAttribute("WCS", "size", 24) # Add text messages component for collision check and robot position vis.addText("textCol", "No collision") vis.setAttribute("textCol", "size", 24) vis.addText("textStep", "Steps: ") vis.setAttribute("textStep", "size", 24) vis.addText("textGoalDistance", "Goal Distance: ") vis.setAttribute("textGoalDistance", "size", 24) vis.addText("textConfig", "Robot configuration: ") vis.setAttribute("textConfig", "size", 24) self.collision_checker = collide.WorldCollider(self.world) vis.setWindowTitle("Simulator") vis.show() return vis, self.robot, self.world
def run_poking(config): """ this is poking api entrance. """ # init params tableHeight = config.tableHeight probeLength = config.probeLength forceLimit = config.forceLimit dt=config.dt #250Hz moveStep=0.002*dt #2mm /s shortServoTime=config.shortServoTime longServoTime=config.longServoTime IKErrorTolerence=config.IKErrorTolerence maxDev=config.maxDev EEZLimit=config.EEZLimit intermediateConfig = config.intermediateConfig probe_transform = config.probe_transform point_probe = np.array([[0,0,0,1], [1-probeLength,0,0,1], [0,-1,0,1]]) # means the point in probe coordinate. point_probe_to_local = np.dot(probe_transform, point_probe.T) point_probe_to_local = point_probe_to_local[0:3,:].T point_probe_to_local = point_probe_to_local.tolist() print("[*]Debug: probe coodinate transform to EE:") print(point_probe_to_local) # init robot world = WorldModel() res = world.readFile(config.robot_model_path) robot = world.robot(0) ee_link=config.ee_link_number #UR5 model is 7. link=robot.link(ee_link) CONTROLLER = config.mode collider = collide.WorldCollider(world) print '---------------------model loaded -----------------------------' # visualization vis.add("world",world) # create folder data_folder = config.exp_path+'exp_'+str(config.exp_number)+'/'+config.probe_type if not os.path.exists(data_folder): os.mkdir(data_folder) # begin loop if config.probe_type == 'point': run_poking_point_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime, IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res,robot,link,CONTROLLER,collider,intermediateConfig) elif config.probe_type == 'line': run_poking_line_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime, IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res,robot,link,CONTROLLER,collider,intermediateConfig) elif config.probe_type == 'ellipse': run_poking_ellipse_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime, IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res,robot,link,CONTROLLER,collider,intermediateConfig) else: print('[!]Probe type no exist')
def find_path(self, q_goal, world, robot): # Extract collision detector for attempts at linking goal to RRT. coll_check = collide.WorldCollider(world) # First, try to connect q_goal to a nearby configuration. # node_goal = self.adj_list[self.get_nearest_neighbor(q_goal)][0] # The idea is to look in 8 directions around the q_goal to find nearest neighbors. This is in case an abundance # of neighbors are not reachable from one side. num_segs = 8 nearest_neighbors = self.get_nearest_neighbors_by_angle(q_goal, num_segs) chosen_neighbor = [] # TODO: Try each of the neighbors, in order of closeness... for neighbor_id in nearest_neighbors: neighbor = self.adj_list[neighbor_id][0] # First, rotate so the robot is facing towards q. Test for collision periodically. # Angular interpolation start_t = neighbor.t # The direction pointing from q_near to q goal_dir_t = math.atan2(q_goal[1] - neighbor.y, q_goal[0] - neighbor.x) if goal_dir_t < 0: goal_dir_t = goal_dir_t + 2 * math.pi elif goal_dir_t >= 2 * math.pi: goal_dir_t = goal_dir_t - 2 * math.pi is_success = False t_step = 10 # angle_diff = goal_dir_t - start_t # angle_diff = abs((angle_diff + 180) % 360) - 180 angle_diff = math.atan2(math.sin(goal_dir_t - start_t), math.cos(goal_dir_t - start_t)) curr_t = start_t rads_rotated = 0 has_collided = False while rads_rotated < abs(angle_diff) and not has_collided: rads_rotated = rads_rotated + t_step curr_t = curr_t + np.sign(angle_diff) * t_step if curr_t < 0: curr_t = curr_t + 2 * math.pi elif curr_t >= 2 * math.pi: curr_t = curr_t - 2 * math.pi # This is the last check we need, but don't overshoot it. if rads_rotated > abs(angle_diff): rads_rotated = abs(angle_diff) curr_t = goal_dir_t # Set current configuration in world. robot.setConfig([neighbor.x, neighbor.y, curr_t]) # Check collision for current configuration. obj_colls = coll_check.robotObjectCollisions(world.robot(0)) for i, j in obj_colls: # There is collision with an obstacle! We can't add this to the tree. # TODO: Possibly try the longer rotation if this one fails. has_collided = True return "TRAPPED" # Translate in a straight line incrementally towards x,y of q. Test for collision periodically. # 2D linear interpolation # Compute (x,y) after traveling dist towards q(x,y) x_diff = q_goal[0] - neighbor.x y_diff = q_goal[1] - neighbor.y mag = math.sqrt(x_diff ** 2 + y_diff ** 2) norm = [x_diff / mag, y_diff / mag] step_mag = .02 curr_mag = step_mag curr_vec = [norm[0] * curr_mag, norm[1] * curr_mag] # This is key difference. The desired mag here is the entire distance from neighbor to q_goal. desired_mag = mag has_collided = False while curr_mag < desired_mag and not has_collided: # Compute new (x,y) curr_mag = curr_mag + step_mag if curr_mag > desired_mag: curr_mag = desired_mag curr_vec = [norm[0] * curr_mag, norm[1] * curr_mag] curr_x = curr_vec[0] + neighbor.x curr_y = curr_vec[1] + neighbor.y robot.setConfig([curr_x, curr_y, curr_t]) obj_colls = coll_check.robotObjectCollisions(world.robot(0)) for i, j in obj_colls: # There is collision with an obstacle! We can't add this to the tree. has_collided = True return "TRAPPED" chosen_neighbor = neighbor break # If we're here, the goal was successfully reached from a neighbor. Add goal to the tree. node_goal = self.add_node([q_goal[0], q_goal[1], goal_dir_t], [chosen_neighbor.id]) # LEGACY #node_goal = self.adj_list[self.get_nearest_neighbor(q_goal)][0] prev_lookup = dict() queue = [node_goal.id] prev_lookup[node_goal.id] = -1 visited = [] while queue: parent_node_id = queue.pop(0) neighbor_ids = self.adj_list[parent_node_id][1:] for neighbor_id in neighbor_ids: if neighbor_id not in visited: queue.append(neighbor_id) node = self.adj_list[neighbor_id][0] prev_lookup[node.id] = parent_node_id visited.append(node.id) if node is self.nodes[0]: break path = [] # Now, put together path. curr_node_id = prev_lookup[self.nodes[0].id] while curr_node_id is not -1: curr_node = self.adj_list[curr_node_id][0] path.append([curr_node.x, curr_node.y, curr_node.t]) curr_node_id = prev_lookup[curr_node_id] #self.plot_path(path) return path
def __init__(self,world): vis.GLPluginInterface.__init__(self) self.world = world self.collider = collide.WorldCollider(world) self.quit = False
def __init__(self, fn): ## Creates a world and loads all the items on the command line self.world = WorldModel() self.robotSystemName = 'O' for f in fn: print(f) res = self.world.readFile(f) if not res: raise RuntimeError("Unable to load model " + fn) self.showVis = False coordinates.setWorldModel(self.world) vis.lock() bW.getDoubleRoomWindow(self.world, 8, 8, 1) vis.unlock() ## Add the world to the visualizer vis.add("world", self.world) vp = vis.getViewport() vp.w, vp.h = 1800, 800 vis.setViewport(vp) self.robots = [] self.n = self.world.numRobots() for i in range(self.n): self.robots.append( sphero6DoF(self.world.robot(i), self.world.robot(i).getName())) self.eps = 0.000001 self.sj = [[0, 0, 0], [0.2, 0, 0]] self.sjStar = [[-0.070534, -0.097082, 0], [0, -0.06, 0], [0.070534, -0.097082, 0], [0.057063, -0.018541, 0], [0.11413, 0.037082, 0], [0.035267, 0.048541, 0], [0, 0.12, 0], [-0.035267, 0.048541, 0], [-0.11413, 0.037082, 0], [-0.057063, -0.018541, 0]] self.sjL = [[0, 0, 0], [0, 0.2, 0], [0, 0.4, 0], [0, 0.6, 0], [0, 0.8, 0], [0, 1, 0], [0.2, 0, 0], [0.4, 0, 0], [0.6, 0, 0], [0.8, 0, 0]] self.sj = self.sjL self.xB = [-4, 4] self.yB = [-4, 4] self.zB = [0.02, 1] self.rad = 0.04 self.currConfig = [0, 0, 1, 1, 0] self.scMin = 1 self.scXMin = 1 self.scYMin = 2 self.sumDist = 0 if self.n > 1: minSij = vectorops.norm(vectorops.sub(self.sj[0], self.sj[1])) minSijX = math.fabs(self.sj[0][0] - self.sj[1][0]) minSijY = math.fabs(self.sj[0][1] - self.sj[1][1]) for i in range(self.n): for j in range(self.n): if i != j: dist = vectorops.norm( vectorops.sub(self.sj[i], self.sj[j])) if dist < minSij: minSij = dist dist = math.fabs(self.sj[i][0] - self.sj[j][0]) if dist < minSijX: minSijX = dist dist = math.fabs(self.sj[i][1] - self.sj[j][1]) if dist < minSijY: minSijY = dist for i in range(self.n): self.sumDist += vectorops.norm(self.sj[i]) if minSij > self.eps: self.scMin = 2 * math.sqrt(2) * self.rad / minSij if minSijX > self.eps: self.scXMin = 2 * math.sqrt(2) * self.rad / minSijX if minSijY > self.eps: self.scYMin = 2 * math.sqrt(2) * self.rad / minSijY self.scMax = max(2, self.scMin) self.scB = [self.scMin, 2 * self.scMin] print('Minimum scale') print(self.scMin) vis.add(self.robotSystemName, [so3.identity(), [10, 10, -10]]) vis.setAttribute(self.robotSystemName, "size", 12) vis.edit(self.robotSystemName) vis.add("WCS", [so3.identity(), [0, 0, 0]]) vis.setAttribute("WCS", "size", 32) vis.edit("WCS") self.collisionChecker = collide.WorldCollider(self.world) if self.showVis: ## Display the world coordinate system vis.addText("textCol", "No collision") vis.setAttribute("textCol", "size", 24) ## On-screen text display vis.addText("textConfig", "Robot configuration: ") vis.setAttribute("textConfig", "size", 24) vis.addText("textbottom", "WCS: X-axis Red, Y-axis Green, Z-axis Blue", (20, -30)) print "Starting visualization window#..." ## Run the visualizer, which runs in a separate thread vis.setWindowTitle("Visualization for kinematic simulation") vis.show() simTime = 60 startTime = time.time() oldTime = startTime self.setConfig(0, 0, 1, 1, 0) q = self.robots[0].getConfig() if self.showVis: q2f = ['{0:.2f}'.format(elem) for elem in q] strng = "Robot configuration: " + str(q2f) vis.addText("textConfig", strng) colFlag = self.checkCollision() print(colFlag) if self.showVis: time.sleep(10)