• API
• FAQ
• Tools
• Archive
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.
Top