SHARE
TWEET

gogurt

a guest Feb 17th, 2020 99 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. def doesArmTouchObjects(armPosDist, objects, isGoal=False):
  2.     """Determine whether the given arm links touch any obstacle or goal
  3.  
  4.        Args:
  5.            armPosDist (list): start and end position and padding distance of all arm links [(start, end, distance)]
  6.            objects (list): x-, y- coordinate and radius of object (obstacles or goals) [(x, y, r)]
  7.            isGoal (bool): True if the object is a goal and False if the object is an obstacle.
  8.                           When the object is an obstacle, consider padding distance.
  9.                           When the object is a goal, no need to consider padding distance.
  10.        Return:
  11.            True if touched. False if not.
  12.    """
  13.     if isGoal == False:
  14.         for obstacle in objects:
  15.             for armL in armPosDist:
  16.                 startX = armL[0][0] - obstacle[0]
  17.                 startY = armL[0][1] - obstacle[1]
  18.                 endX = armL[1][0] - obstacle[0]
  19.                 endY = armL[1][1] - obstacle[1]
  20.                 # quadratic equation combining parametric eq of line and circle eq
  21.                 A = (endX - startX)**2 + (endY - startY)**2
  22.                 B = 2*(startX*(endX - startX) + startY*(endY - startY))
  23.                 C = startX**2 + startY**2 - (obstacle[2] + armL[2])**2
  24.                 # AX^2 + BX + C
  25.                 discriminant = (B**2) - (4*A*C)
  26.                 # no real roots b/c sqrt in quadratic formula will be sq root of negative
  27.                 # print(discriminant)
  28.                 if discriminant <= 0:
  29.                     continue
  30.                 # now solve for parametric roots
  31.                 tIntersect1 = (-B + math.sqrt(discriminant))/(2*A)
  32.                 tIntersect2 = (-B - math.sqrt(discriminant))/(2*A)
  33.                 # now we will check if t is within the constraint values for the parametric line eq
  34.                 if (tIntersect1 >= 0 and tIntersect1 <= 1) or (tIntersect2 >= 0 and tIntersect2 <=1):
  35.                     return True
  36.  
  37.     else:
  38.         for goal in objects:
  39.             for armL in armPosDist:
  40.                 startX = armL[0][0] - goal[0]
  41.                 startY = armL[0][1] - goal[1]
  42.                 endX = armL[1][0] - goal[0]
  43.                 endY = armL[1][1] - goal[1]
  44.                 # quadratic equation combining parametric eq of line and circle eq
  45.                 A = (endX - startX)**2 + (endY - startY)**2
  46.                 B = 2*(startX*(endX - startX) + startY*(endY - startY))
  47.                 C = startX**2 + startY**2 - (goal[2])**2
  48.                 # AX^2 + BX + C
  49.                 discriminant = (B**2) - (4*A*C)
  50.                 # no real roots b/c sqrt in quadratic formula will be sq root of negative
  51.                 # print(discriminant)
  52.                 if discriminant <= 0:
  53.                     continue
  54.                 # now solve for parametric roots
  55.                 tIntersect1 = (-B + math.sqrt(discriminant))/(2*A)
  56.                 tIntersect2 = (-B - math.sqrt(discriminant))/(2*A)
  57.                 # now we will check if t is within the constraint values for the parametric line eq
  58.                 if (0 <=tIntersect1  and tIntersect1 <= 1) or (0 <= tIntersect2  and tIntersect2 <=1):
  59.                     return True
  60.  
  61.     return False
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top