def checkStartGoal(start, goal, boxes):
isCollided = False
for box in boxes:
isCollided = isCollided or np.any(detectCollision(start, start, box))
isCollided = isCollided or np.any(detectCollision(goal, goal, box))
if isCollided:
return isCollided
return isCollided
def obstacleCollision(start, goal, obstacles):
"""
start is a pose containing q1 -> qe
goals is a pose containing q1 -> qe
returns if there is a feasible straight line path
"""
lineCollision = False
# print("================ Obstacle Collision ================")
# print("lineCollision is " + str(lineCollision))
# print("Start " + str(start))
# print("Goal " + str(goal))
f = calculateFK()
# All the joint XYZ values
# Use index 0 for start and goal because they are
# nested lists
startPos, _ = f.forward(start[0])
goalPos, _ = f.forward(goal[0])
# print("StartPos " + str(startPos))
# print("GoalPos " + str(goalPos))
for obstacle in obstacles:
# Iterate through every joint
for i in range(len(startPos)):
results = detectCollision([startPos[i]], [goalPos[i]], obstacle)
for result in results:
lineCollision |= result
# print("lineCollision is " + str(lineCollision))
return lineCollision
def isCollided(pts1, pts2, boxes):
isCollided = False
next_joint_idx = [1, 2, 3, 4, 5]
link_pts_1 = pts1[next_joint_idx, :]
link_pts_2 = pts2[next_joint_idx, :]
# Check self collision
rs = link_pts_2 - pts2[:5, :]
for i in range(len(rs) - 1):
p = pts2[i]
r = rs[i]
for j in range(i + 1, len(rs)):
q = pts2[j]
s = rs[j]
rxs = np.cross(r, s).sum()
qpxr = np.cross(q - p, r).sum()
if rxs == 0 and qpxr == 0 and np.dot(s, r) < 0:
t0 = np.dot(q - p, r) / np.dot(r, r)
t1 = np.dot(q + s - p, r) / np.dot(r, r)
if not (t1 > 1 or t0 < 0):
return True
elif rxs != 0 and i < 2:
t = np.cross(q - p, s).sum() / np.cross(r, s).sum()
u = np.cross(q - p, r).sum() / np.cross(r, s).sum()
if 0 < t < 1 and 0 < u < 1:
return True
# Check external collision
pts1 = np.vstack((pts1, pts1[:5, :], link_pts_2))
pts2 = np.vstack((pts2, link_pts_1, pts2[:5, :]))
for box in boxes:
isCollided = np.any(detectCollision(pts1, pts2, box))
if isCollided:
return isCollided
return isCollided
def process_index(self, ind):
"""
Calculates if the metric described by this index is occupied
:param ind:
:return:
"""
beg_index = [0, 1, 2, 3, 4]
end_index = [1, 2, 3, 4, 5]
joint_vector = self.index_to_metric_negative_corner(ind)
joint_pos, foo = self.FK.forward(np.append(joint_vector, [0, 0]))
beg_points = joint_pos[beg_index, ::]
end_points = joint_pos[end_index, ::]
for block in self.InflatedBlocks:
is_collided = detectCollision(beg_points, end_points, block)
if any(is_collided):
self.occ[ind[0]][ind[1]][ind[2]][ind[3]] = True
break
self.occ_check[ind[0]][ind[1]][ind[2]][ind[3]] = True
def isValidConfig(q, obstacles):
"""
Check if a configuration is in free C-space
:q: Configuration of the robot (1x6)
:obstacles location of all boxes (Nx6) [xmin, ymin, zmin, xmax, ymax, zmax]
:return: True if valid configuration, else false
"""
fk = calculateFK()
q = np.reshape(q, -1)
points, T0e = fk.forward(q)
buffer = 5 # buffer to add to all links
# Define the space of link 1: ####################################
delta1 = 50 + buffer # half the depth of link 1, mm
w1 = 50 + buffer # half the width of link 1, mm
link1 = makeRectangle(points, q, 1, delta1, w1) # rectangle at joint 1
# Define the space of link 2: ####################################
delta2 = 10.3 + buffer
w2 = 33.3 + buffer
link2 = makeRectangle(points, q, 2, delta2, w2)
# Define the space of link 3: ####################################
delta3 = 15.5 + buffer
w3 = 24.5 + buffer
# define special case for the bit of the lynx sticking out its back:
l = points[3, :] - points[2, :]
l = l / np.linalg.norm(l) # unit vector pointing in direction of link 3
l = l * (-28.5) # rough distance to the back end
link3Points = np.array([points[2, :] + l, points[3]])
link3 = makeRectangle(link3Points, q, 1, delta3, w3)
# Define the space of link 4: ####################################
delta4 = 15.5 + buffer
w4 = 30.1 + buffer
link4 = makeRectangle(points, q, 4, delta4, w4)
# Define the space of link 5: ####################################
delta5 = 27.3 + buffer
w5 = 10.5 + buffer
link5 = makeRectangle(points, q, 5, delta5, w5, rot=q[4])
# Define the space of end effector: ##############################
if (q[5] >= 0):
deltaEE = 5.2 + q[5] + buffer
else:
deltaEE = 5.2 + buffer
wEE = 10.5 + buffer
# have to make a new point for the tip of the end effector:
l = points[-1, :] - points[4, :]
l = l / np.linalg.norm(
l) # the unit vector pointing in the direction of the ee
l = l * 12.5 + buffer # the length of the end effector tips
eePoints = np.array([points[-1, :], points[-1, :] + l])
linkEE = makeRectangle(eePoints, q, 1, deltaEE, wEE,
rot=q[4]) # use end effector points
# the index of the points in the rectangles that begin and end a line
# the first four pairs are the lines along the length of the rectangle
# the last eight pairs are the rectangles at the end of the prism
startPtIdx = np.array([0, 1, 2, 3, 0, 1, 2, 3, 4, 5, 6, 7], dtype=np.int8)
endPtIdx = np.array([4, 5, 6, 7, 1, 2, 3, 0, 5, 6, 7, 4], dtype=np.int8)
# Test to see if any links collide with an obstacle
for link in np.array([link1, link2, link3, link4, link5, linkEE]):
for obs in range(len(obstacles)):
isCollide = detectCollision(link[startPtIdx], link[endPtIdx],
obstacles[obs])
if (any(isCollide)):
return False
for link in np.array([link3, link4, link5, linkEE]):
isCollide = detectCollision(link[startPtIdx], link[endPtIdx],
np.array([-50, -50, 0, 50, 50, 76.2]))
if (any(isCollide)):
print("Lynx collides with base frame")
return False
return True