def path_in_collision(q1, q2, obstacles_segs): endpoint1 = fk(q1) endpoint2 = fk(q2) for obstacle in obstacle_segs: if collision.intersect(endpoint1, endpoint2, obstacle[0], obstacle[1]): return True return False
def rrt(target_x, q0, NIter = 10000, pub = None, vis_pub= None): nodes = [ Node(q0) ] start_x = fk(q0) if vis_pub is not None: vis_pub.publish(createSphereMarker(2, namespace="", pose = [target_x[0], 0, target_x[1], 0, 0, 0 ,1], rgba=(0,0,1,1), frame_id = '/arm_base')) rospy.sleep(0.2) vis_pub.publish(createSphereMarker(3, namespace="", pose = [start_x[0], 0, start_x[1], 0, 0, 0 ,1], rgba=(1,0,1,1), frame_id = '/arm_base')) rospy.sleep(0.2) for i in xrange(NIter): if i % 100 == 0: print 'iteration:', i rospy.sleep(0.01) # pick a node in work space randomly q_rand = [ np.random.uniform(joint_limits[0][0], joint_limits[0][1]), np.random.uniform(joint_limits[1][0], joint_limits[1][1]) ] nearest_node_index = find_nearest_node (nodes, q_rand) new_q = step_from_toward(nodes[nearest_node_index].q, q_rand) #print 'new_q', new_q #print 'in_workspace(new_q)', in_workspace(new_q) #print 'in_collision(new_q)', in_collision(new_q) if pub is not None: js = sensor_msgs.msg.JointState(name=['joint1', 'joint2'], position = (new_q[0], new_q[1])) pub.publish(js) if in_workspace(new_q) and not collision.in_collision(new_q, obstacle_segs) and not path_in_collision(new_q, nodes[nearest_node_index].q, obstacle_segs): if vis_pub is not None: xz = fk(new_q) xz_old = fk(nodes[nearest_node_index].q) vis_pub.publish(createPointMarker([[xz[0], 0, xz[1]]], i+6, namespace="", rgba=(0,1,0,1), frame_id = '/arm_base')) vis_pub.publish(createLineStripMarker([[xz_old[0], 0, xz_old[1]] , [xz[0], 0, xz[1]]], marker_id = i+200000, rgba = (0,0.5,0,1), pose=[0,0,0,0,0,0,1], frame_id = '/arm_base')) new_node = Node(new_q, nearest_node_index) nodes.append(new_node) if dist(fk(new_q), target_x) < TARGET_RADIUS: plan = backtrace(nodes, new_node) return plan return None # no plan found
def in_collision(q, obstacle_segs): arm_segments = [( (0,0), fk1(q) ), ( fk1(q), fk(q))] for seg1 in arm_segments: for seg2 in obstacle_segs: if intersect(seg1[0], seg1[1], seg2[0], seg2[1]): return True return False
def in_collision(q, obstacle_segs): arm_segments = [((0, 0), fk1(q)), (fk1(q), fk(q))] for i in obstacle_segs: for j in arm_segments: if intersect(i[0], i[1], j[0], j[1]): return True return False
def in_collision(q, obstacle_segs): arm_segments = [( (0,0), fk1(q) ), ( fk1(q), fk(q))] final_val = False for obs in obstacle_segs: for arm in arm_segments: inter = intersect(arm[0],arm[1],obs[0],obs[1]) final_val = final_val or inter return final_val
def in_collision(q, obstacle_segs): arm_segments = [( (0,0), fk1(q) ), ( fk1(q), fk(q))] incollision = False for seg in obstacle_segs: for arm in arm_segments: if intersect(arm[0],arm[1],seg[0],seg[1]): incollision = True break return incollision
def in_workspace(q): x = fk(q) return x[0] >= 0 and x[0] <= XDIM and x[1] >= 0 and x[1] <= ZDIM