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
