def solverBelt(pos, rot, object_type): X_PIECE = pos[0] Y_PIECE = pos[1] Z_PIECE = pos[2] object_height = OBJECT_HEIGHT[object_type.upper()] if object_type.upper() in OBJECT_HEIGHT else 0 Z_PIECE+=object_height T2 = (X_PIECE - X_BASE) - H_WRIST H1 = 0.128 - ((GRIPPER + WRIST_1_2) - (Z_BASE - Z_PIECE)) _,_,T3,A1,B1,_ = solve(a=T2, b=H1, C=90*degree) _,_,_,A2,B2,C2 = solve(a=T3, b=UP_ARM, c=FORE_ARM) ##--------------- Angle Conversion -------------## angle_elbow = pi - A2 angle_shoulder = (pi/2) - (A1 + C2) angle_wrist = (pi/2) + (pi - (B1+B2)) angles = [angle_elbow, Y_PIECE - WRIST_LENGTH, angle_shoulder, 0.0, angle_wrist, -1.57, -rot[2]-1.57] # angles = [2.51, Y_PIECE - WRIST_LENGTH, -1.76, 0, 3.69, -1.56, 0.0] return angles
def computeToolTipGoal(pos, currJoints): X_PIECE = pos[0] Y_PIECE = pos[1] Z_PIECE = pos[2] T2 = (X_BASE - X_PIECE) - H_WRIST line_arm_actuator = Y_PIECE + WRIST_LENGTH H1 = ((Z_BASE - Z_PIECE) - (GRIPPER + WRIST_1_2)) + H_BASE _,_,T3,A1,B1,_ = solve(a=T2, b=H1, C=90*degree) _,_,_,A2,B2,C2 = solve(a=T3, b=UP_ARM, c=FORE_ARM) ##--------------- Angle Conversion -------------## angle_elbow = pi - A2 angle_shoulder = (pi/2) - (A1 + C2) angle_wrist = (pi/2) + (pi - (B1+B2)) angles = [angle_elbow, currJoints[1], angle_shoulder, currJoints[3], angle_wrist, currJoints[5], currJoints[6]] return angles
def moveUp(pos, object_type): X_PIECE = pos[0] Y_PIECE = pos[1] Z_PIECE = pos[2] #object_height = OBJECT_HEIGHT[object_type.upper()] #Z_PIECE+=object_height T2 = (X_BASE - X_PIECE) - H_WRIST line_arm_actuator = Y_PIECE + WRIST_LENGTH H1 = ((Z_BASE - Z_PIECE) - (GRIPPER + WRIST_1_2)) + H_BASE _,_,T3,A1,B1,_ = solve(a=T2, b=H1, C=90*degree) _,_,_,A2,B2,C2 = solve(a=T3, b=UP_ARM, c=FORE_ARM) ##--------------- Angle Conversion -------------## angle_elbow = pi - A2 angle_shoulder = (pi/2) - (A1 + C2) angle_wrist = (pi/2) + (pi - (B1+B2)) angles = [angle_elbow + 0.21, line_arm_actuator, angle_shoulder-0.11, 3.14, angle_wrist, -1.57, 0.0] return angles
def solverBin(pos, rot, object_type, ignore_height=False): rospy.loginfo( '[solverBin] pos: {}; rot: {}; object_type {}; ignore_height: {}'.format( pos, rot, object_type, ignore_height)) X_PIECE = pos[0] Y_PIECE = pos[1] Z_PIECE = pos[2] object_height = OBJECT_HEIGHT[object_type.upper()] if object_type.upper() in OBJECT_HEIGHT else 0 if(not ignore_height): Z_PIECE+=object_height T2 = (X_BASE - X_PIECE) - H_WRIST line_arm_actuator = Y_PIECE + WRIST_LENGTH H1 = ((Z_BASE - Z_PIECE) - (GRIPPER + WRIST_1_2)) + H_BASE _,_,T3,A1,B1,_ = solve(a=T2, b=H1, C=90*degree) _,_,_,A2,B2,C2 = solve(a=T3, b=UP_ARM, c=FORE_ARM) ##--------------- Angle Conversion -------------## angle_elbow = pi - A2 angle_shoulder = (pi/2) - (A1 + C2) angle_wrist = (pi/2) + (pi - (B1+B2)) angles = [angle_elbow, line_arm_actuator, angle_shoulder, 3.14, angle_wrist, -1.57, 1.57 - rot[2]] return angles
def compute_slacks(height, width, hyp_ratio) -> Tuple[float, float]: """Compute slacks to add to bounding box on each site""" # compute angle and side for hypotenuse _, b, _, A, _, _ = solve(c=width, a=height, B=pi / 2) # compute new height and width a, _, c, _, _, _ = solve(b=b * (1.0 + hyp_ratio), B=pi / 2, A=A) # compute slacks return c - width, a - height
def mb_mfb(ang_in_degrees, joint_string): KNIE = { 'MA': 54, # 3 'AB': 85.85, # 4 'FB': 34.6, # 3 'MF': 86.0 # 4 } ENKEL = { 'MA': 36.5, # 3 'AB': 89.4, # 4 'FB': 14.5, # 3 'MF': 117 # 4 } if joint_string.lower() == 'enkel': joint = ENKEL elif joint_string.lower() == 'knie': joint = KNIE else: raise Exception('%s is not a known joint' % joint_string) # First solve the bottom triangle: # A # |\ # c | \ b # | \ # B---C # a # B is given and c and a is known MA = joint.get('MA') AB = joint.get('AB') FB = joint.get('FB') MF = joint.get('MF') ang = math.radians(ang_in_degrees) a, b, c, A, B, C = solve(c=MF, a=FB, B=ang) # Then solve the top triangle # c # A---B # \ | # b \ | a # \| # C # a, b and c is know a1, b1, c1, A1, B1, C1 = solve(a=AB, b=b, c=MA) # Lock # a2, b2, c2, A2, B2, C2 = solve(a=AB+FB, b=MF, c=MA) return math.degrees(A + A1)
def depositOnTray1(pos, rot, object_type, ignore_height=False, adjust=False): X_PIECE = pos[0] Y_PIECE = pos[1] Z_PIECE = pos[2] object_height = OBJECT_HEIGHT[object_type.upper()] if object_type.upper() in OBJECT_HEIGHT else 0 Z_PIECE+=object_height # if object_type == "pulley_part": # object_height = OBJECT_HEIGHT[object_type.upper()] # Z_PIECE+=((object_height*2)) # else: # object_height = OBJECT_HEIGHT[object_type.upper()] # Z_PIECE+=object_height angle_var = (atan2(X_PIECE - X_BASE,Y_PIECE - Y_BASE) + atan2(WRIST_LENGTH,Y_PIECE - Y_BASE)) angle_shoulder_pan = 90*degree - angle_var dx_base, dy_base = point_pos(X_BASE, Y_BASE, BASE_UPPER_DIST, angle_var, 1) T1 = sqrt((Y_PIECE - dy_base)**2 + (X_PIECE - dx_base)**2) T2 = T1 - H_WRIST h_base_piece = Z_BASE - Z_PIECE h_base_imaginary = (GRIPPER + WRIST_1_2) - h_base_piece H1 = H_BASE - h_base_imaginary rospy.loginfo( '[depositOnTray1] dx_base:{}; dy_base={}; angle_var: {}; angle_var_rad: {}; T1: {}; T2: {}; h_base_piece {}; h_base_imaginary: {}; H1: {}; adhust: {}'.format( dx_base, dy_base, angle_var/degree, angle_var, T1, T2, h_base_piece, h_base_imaginary, H1, adjust)) _,_,T3,A1,B1,_ = solve(a=T2, b=H1, C=90*degree) _,_,_,A2,B2,C2 = solve(a=T3, b=UP_ARM, c=FORE_ARM) ##--------------- Angle Conversion -------------## angle_elbow = pi - A2 angle_shoulder_lift = (pi/2) - (A1 + C2) angle_wrist = (pi/2) + (pi - (B1+B2)) if adjust: angles = [angle_elbow, 2.10, angle_shoulder_lift, angle_shoulder_pan, angle_wrist, -1.57, -rot[2]-angle_var] else: angles = [angle_elbow, 2.10, angle_shoulder_lift, angle_shoulder_pan, angle_wrist, -1.57, 3.1415 - rot[2] - angle_var] return angles
def moveToolTip(incrementZ=0.3, incrementX=0.1, timeToGoal=0.2, accError=[ 0.009, 0.009, 0.009, 0.009, 0.015, 0.015, 0.009, 0.009, 0.009 ]): posUpperArm, angleUpperArm = utils.getUpperArmPose() posVacum, angleVacum = utils.getVacuumGripperPos() posFore, angleFore = utils.getForeArmPos() workingPos = deepcopy(posVacum) workingPos[2] += incrementZ workingPos[0] += incrementX shoulderBTriangle = utils.computeXZDistance(posUpperArm, workingPos) xDistance = abs(utils.computeXDistance(posUpperArm, workingPos)) zDistance = abs(utils.computeZDistance(posUpperArm, workingPos)) wristBTriangle = utils.computeXZDistance(workingPos, posVacum) workingIsAbove = utils.computeZDistance(posUpperArm, workingPos) < 0 a1, b1, c1, A1, B1, C1 = solve(a=shoulderBTriangle, b=FORE_ARM, c=UP_ARM) a2, b2, c2, A2, B2, C2 = solve(a=xDistance, b=zDistance, c=shoulderBTriangle) elbow_joint = math.pi - A1 shoulder_lift_joint = math.pi / 2 - A2 - B1 wrist_1_joint = math.pi - (C1 + B2) + math.pi / 2 angles = [] angles.extend(global_vars.current_joint_state.position) angles[0] = elbow_joint angles[2] = shoulder_lift_joint angles[4] = wrist_1_joint # rospy.sleep(1) set_arm_joint_values(angles, timeToGoal) check_arm_joint_values_published(list_of_joint_values=angles, accError=accError)
def cosine_angle(a, b, c): """ the angles of a triangle if one knows the three sides theta = acos( ( a**2 + b**2 + x**2 ) / 2ab ) ================= //\\ //--\\ a // \\ b //______\\ c ================ """ """ #print("a = ",a,"b = ",b,"c = ",c) tmp = a**2 + b**2 - c**2 #print("tmp = ",tmp) dn = 2 * a * b #print("dn = ",dn) tmp = tmp / float(dn) if tmp > 1.0000000000000000000000000000000000000000000000000000000000000000000000 or \ tmp < -1.000000000000000000000000000000000000000000000000000000000000000000000 : tmp = round(tmp) #print("tmp = ",tmp) # return math.degrees(math.acos(tmp))""" try: a, b, c, A, B, C = solve(a=a, b=b, c=c) except AssertionError: return None return C / degree
def trajectory(acc, angle): acc = abs(acc) angle = abs(acc) a,b,c,A,B,C = solve (b = acc, c = acc, A = angle*degree) foundLength = a otherAngle = (180 - angle)/2 a,b,c,A,B,C = solve (a = foundLength, B = otherAngle*degree, A = 90*degree) #working out y movement y = acc - c #working out x movement x = b return(x, y)
def moveToolTipZY(incrementZ=0.3, incrementY=0.1, timeToGoal=0.2): posUpperArm, angleUpperArm = utils.getUpperArmPose() posVacum, angleVacum = utils.getVacuumGripperPos() posFore, angleFore = utils.getForeArmPos() workingPos = deepcopy(posVacum) workingPos[2] += incrementZ workingPos[1] += incrementY shoulderBTriangle = utils.computeYZDistance(posUpperArm, workingPos) yDistance = abs(utils.computeYDistance(posUpperArm, workingPos)) zDistance = abs(utils.computeZDistance(posUpperArm, workingPos)) wristBTriangle = utils.computeYZDistance(workingPos, posVacum) workingIsAbove = utils.computeZDistance(posUpperArm, workingPos) < 0 a1, b1, c1, A1, B1, C1 = solve(a=shoulderBTriangle, b=FORE_ARM, c=UP_ARM) a2, b2, c2, A2, B2, C2 = solve(a=yDistance, b=zDistance, c=shoulderBTriangle) elbow_joint = math.pi - A1 shoulder_lift_joint = math.pi / 2 - A2 - B1 wrist_1_joint = math.pi - (C1 + B2) + math.pi / 2 angles = [] angles.extend(global_vars.current_joint_state.position) angles[0] = elbow_joint angles[2] = shoulder_lift_joint angles[4] = wrist_1_joint # rospy.sleep(1) # msg = utils.createJointTrajectory(angles, time=1) # joint_trajectory_publisher.publish(msg) set_arm_joint_values(angles, timeToGoal) check_arm_joint_values_published(list_of_joint_values=angles)
def solveKinematicsFront(eezL, eezR, foot_dist): # stage 1 a, b, c, A, B, C = solve(a=foot_dist, c=(eezR + 0.115), B=90 * degree) b1 = b A1 = A / degree C1 = C / degree # stage 2 a, b, c, A, B, C = solve(a=foot_dist, b=b1, c=(eezL + 0.115)) A2 = A / degree C2 = C / degree # stage 3 foot_angle = 180 - (A1 + C2) # drawRadarFront(foot_angle_R, foot_angle_L, eezL, eezR, foot_dist, "green") return foot_angle
def solveKinematics(ee_z, ee_x, lnk_1, lnk_2): joint_2 = 0 side_c1 = 0 angle_a1 = 0 angle_a2 = 0 angle_b1 = 0 angle_b2 = 0 pos = True if ee_x < 0: pos = False a,b,c,A,B,C = solve(a=ee_z, b=abs(ee_x), C=90*degree) side_c1 = c angle_a1 = A / degree angle_b1 = B / degree if side_c1 > (lnk_1 + lnk_2): print("Target out of reach!") else: a,b,c,A,B,C = solve(a=lnk_1, b=lnk_2, c=side_c1) # print("Knee angle: ", round(C / degree, 2)) angle_a2 = A / degree angle_b2 = B / degree meta_a = (angle_a1 + angle_a2) meta_b = (angle_b1 + angle_b2) joint_2 = C / degree # print("Thigh angle: ", (180 - meta_b)) # print("Foot angle: ", (180 - meta_a)) # drawRadar(meta_a, meta_b, joint_2, lnk_1, lnk_2, pos) return [meta_a, meta_b, joint_2, pos]
def solveKinematicsNeg(eez, eex, lnk1, lnk2): # stage 1 a, b, c, A, B, C = solve(a=abs(eex), c=eez, B=90 * degree) b1 = b A1 = A / degree C1 = C / degree # stage 2 a, b, c, A, B, C = solve(a=lnk2, b=b1, c=lnk1) A2 = A / degree B2 = B / degree C2 = C / degree # stage 3 meta_a = ((C1 + A2) - 90) meta_b = (180 - B2) meta_c = (A1 + C2) # drawRadar(meta_a, meta_b, meta_c, lnk1, lnk2, "red") return [meta_a, meta_b, meta_c]
def checkValidity(a: float, b: float, c: float) -> bool: """ Triangle rule checks it's triangle edged as a, b, c Args: a: edge b: edge c: edge Returns: if is not triangle and C degree over the defined variable return False. """ a, b, c, A, B, C = solve(a, b, c) if (a + b <= c) or (a + c <= b) or (b + c <= a): if not 5 < (C / degree) < 20: return False return True
def measurements(self) -> Measurements: """ Calcular as medidas absolutas do triângulo, a partir dos dados incompletos fornecidos. """ a, b, c, aA, aB, aC = trianglesolver.solve( a=self._data.get('side_a'), b=self._data.get('side_b'), c=self._data.get('side_c'), A=self._data.get('angle_a'), B=self._data.get('angle_b'), C=self._data.get('angle_c') ) return Measurements( side_a=a, side_b=b, side_c=c, angle_a=aA, angle_b=aB, angle_c=aC )
def checkValidity(a, b, c): """ Triangle rule checks it's triangle edged as a, b, c Parameters ---------- a b c Returns if is not triangle and C degree over the defined variable return False. ------- """ a, b, c, A, B, C = solve(a, b, c) if (a + b <= c) or (a + c <= b) or (b + c <= a): if not 5 < (C / degree) < 20: return False return True
def cosine_length(C, a, b): a, b, c, A, B, C = solve(a=a, b=b, C=C * degree) return c
from trianglesolver import solve, degree from advents_of_code.year2019.day18.intcode import OutputInterrupt, IntCode program = """109,424,203,1,21102,11,1,0,1105,1,282,21101,18,0,0,1106,0,259,1202,1,1,221,203,1,21101,0,31,0,1106,0,282,21102,38,1,0,1105,1,259,20101,0,23,2,22102,1,1,3,21102,1,1,1,21102,57,1,0,1105,1,303,2101,0,1,222,21002,221,1,3,20101,0,221,2,21102,1,259,1,21101,0,80,0,1105,1,225,21102,40,1,2,21101,0,91,0,1105,1,303,1201,1,0,223,20101,0,222,4,21101,0,259,3,21101,0,225,2,21101,0,225,1,21102,118,1,0,1105,1,225,21001,222,0,3,21102,1,144,2,21101,0,133,0,1105,1,303,21202,1,-1,1,22001,223,1,1,21102,148,1,0,1105,1,259,1202,1,1,223,20101,0,221,4,21001,222,0,3,21102,1,14,2,1001,132,-2,224,1002,224,2,224,1001,224,3,224,1002,132,-1,132,1,224,132,224,21001,224,1,1,21102,195,1,0,106,0,109,20207,1,223,2,20101,0,23,1,21101,0,-1,3,21102,214,1,0,1105,1,303,22101,1,1,1,204,1,99,0,0,0,0,109,5,2101,0,-4,249,22101,0,-3,1,21201,-2,0,2,22101,0,-1,3,21101,0,250,0,1105,1,225,21202,1,1,-4,109,-5,2105,1,0,109,3,22107,0,-2,-1,21202,-1,2,-1,21201,-1,-1,-1,22202,-1,-2,-2,109,-3,2106,0,0,109,3,21207,-2,0,-1,1206,-1,294,104,0,99,21202,-2,1,-2,109,-3,2106,0,0,109,5,22207,-3,-4,-1,1206,-1,346,22201,-4,-3,-4,21202,-3,-1,-1,22201,-4,-1,2,21202,2,-1,-1,22201,-4,-1,1,21202,-2,1,3,21101,343,0,0,1106,0,303,1105,1,415,22207,-2,-3,-1,1206,-1,387,22201,-3,-2,-3,21202,-2,-1,-1,22201,-3,-1,3,21202,3,-1,-1,22201,-3,-1,2,21202,-4,1,1,21102,384,1,0,1106,0,303,1105,1,415,21202,-4,-1,-4,22201,-4,-3,-4,22202,-3,-2,-2,22202,-2,-4,-4,22202,-3,-2,-3,21202,-4,-1,-2,22201,-3,-2,1,22101,0,1,-4,109,-5,2106,0,0""" a, b, c, A, B, C = solve(C=90 * degree, b=80, A=32.784 * degree) def get_wall(x, y): comp = IntCode(program=program) comp.input_queue.append(y) comp.input_queue.append(x) while not comp.done: try: comp.run() except OutputInterrupt: pass # print("Sensor BOOST code: ", comp.output_queue[-1]) if comp.output_queue[-1]: return '#' else: return '.' x, y = 772, 1507 for x in range(772, 750, -1): for y in range(1507, 1380, -1):
def sinCosTan(lnk_1, lnk_2, hypo): a,b,c,A,B,C = solve(a=lnk_1, b=lnk_2, c=hypo) return[(A / degree), (B / degree), (C / degree)]
def rateImage(image_path, num_tumors, verbose=False, plot=False): """ rates a given image by summing up tumor distances and comparing to enclosing circle of liver """ img = cv2.imread(image_path) num_tumors = num_tumors b, g, r = cv2.split(img) # threholding, closing of tumors/liver ret, thresh1 = cv2.threshold(b, 75, 255, cv2.THRESH_BINARY) ret, thresh2 = cv2.threshold(r, 75, 277, cv2.THRESH_BINARY) kernel = np.ones((5, 5), np.uint8) closing = cv2.morphologyEx(thresh1, cv2.MORPH_CLOSE, kernel) closing_liver = cv2.morphologyEx(thresh2, cv2.MORPH_CLOSE, kernel) if plot: plt.subplot(131), plt.imshow(cv2.cvtColor( closing, cv2.COLOR_GRAY2BGR)), plt.title("closing") dst = cv2.addWeighted(img, 0.7, cv2.cvtColor(closing, cv2.COLOR_GRAY2BGR), 0.3, 0.0) _, contours_tumors, _ = cv2.findContours(closing.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) _, contours_liver, _ = cv2.findContours(closing_liver.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) cv2.drawContours(dst, contours_tumors, -1, (255, 0, 0), 1) # sum up liver contours and tumor area area_liver = cv2.contourArea(contours_liver[0]) liverPts = contours_liver[0] for contour in contours_liver[1:]: area_liver += cv2.contourArea(contour) liverPts = np.concatenate((liverPts, contour)) area_tumors = 0 for contour in contours_tumors: area_tumors += cv2.contourArea(contour) # calculate tumor momentums -> centers centers = [] for i in range(len(contours_tumors)): moments = cv2.moments(contours_tumors[i]) # check m00 because division by zero ??? profit has_zero_moments = moments['m00'] == 0.0 if verbose: print moments['m00'] if not has_zero_moments: centers.append((moments['m10'] / moments['m00'], moments['m01'] / moments['m00'])) else: pass # print "a tumor has zero momentum" # check if at least 2 tumors are visible -> else no distance possible! if len(centers) > 1: pts = np.array(centers, np.int32) pts = pts.reshape((-1, 1, 2)) # calculate hull of tumor points hull = cv2.convexHull(pts) cv2.drawContours(dst, [hull], 0, (255, 255, 255)) # calculate enclosing circle of liver (x, y), radius = cv2.minEnclosingCircle(liverPts) center = (int(x), int(y)) radius = int(0.65 * radius) cv2.circle(dst, center, radius, (255, 255, 255), 1) # calculate distances of tumors (arclength) arcLength = cv2.arcLength(hull, True) # calculate max score possible distance angle = 360.0 / num_tumors * degree if verbose: print "calculating maxDistance: a={0}, b={1}, C={2}".format( radius, radius, angle) _, _, maxDistance, _, _, _ = solve(a=radius, b=radius, C=angle) score = arcLength / (maxDistance * num_tumors) # punish views with less tumors punish_factor = len(centers) / float(num_tumors) if len( centers) <= num_tumors else 1 score = score * punish_factor if score > 1: score = 1 else: score = 0 if verbose: print "len(contours_liver)", len(contours_liver) print "Score: ", score print "arcLength: ", arcLength print "Tumorezentren:", centers print "sichtbare Tumore:", len(contours_tumors) print "Leberflaeche: ", area_liver print "Tumorflaeche: ", area_tumors if plot: plt.subplot(132), plt.imshow(cv2.cvtColor( dst, cv2.COLOR_BGR2RGB)), plt.title("dst") plt.subplot(133), plt.imshow(cv2.cvtColor( img, cv2.COLOR_BGR2RGB)), plt.title("img") plt.show() return score, len(centers)
def GenerateOpenSCAD(var, cs, SA, HL, pipe, TU, SD, M, F, T): SEGMENTS = 48 # solidpython variable for midinr in var: inputorder = var.index(midinr) f = midi2freq(int(midinr)) # get frequency by midi nr WL = cs / f # wavelength in mm ILA = WL / 2 # internal length L69 = ((cs / midi2freq(69)) / 2) # internal length of midi nr 69 IW = L69 * 2**((69 - int(midinr)) / M) / 12 # internal width IL = cs / (2 * f) # calculate internal length def openclosedmeasurements(): if pipe == "Closed": l = 0.52 * IL + IW + F mh = (3.018 - (0.233 * (math.log(f))))**5 return l, mh elif pipe == "Open": l = 1.03 * IL - IW + F mh = 550 / 2**(math.log(f)) return l, mh L, MH = openclosedmeasurements() # L = length; MH=Mouth Height a, b, c, A, B, C = solve(a=T - HL, A=SA * degree, B=90 * degree) LL = c # length labium Wb = IW + 2 * T # width back Ws = IW # width sides BPl = F + IW # bottom plug length BPw = IW # bottom plug width BPt = IW # bottom plug thickness Cl = F + IW # cover length Cw = IW + 2 * T # cover width Gl = F + 2 * IW # gasket length Gw = IW + 2 * T # gasket width Fl = L - MH - F - IW # Front length if inputorder == 0: startpoint = 0 scadcode = assembly(startpoint, Wb, L, T, Ws, Cw, Cl, Fl, HL, LL, IW, TU, SD, pipe) startpoint = 2 * Wb + 2 * Ws + IW + 5 * 10 else: scadcode = scadcode.add( assembly(startpoint, Wb, L, T, Ws, Cw, Cl, Fl, HL, LL, IW, TU, SD, pipe)) startpoint = startpoint + (2 * Wb + 2 * Ws + IW + 5 * 10) scad_string_dirty = scad_render(scadcode) scad_string = scad_string_dirty.replace( scad_string_dirty[scad_string_dirty.find("<") + 1:scad_string_dirty.find(">")], basename(scad_string_dirty[scad_string_dirty.find("<") + 1:scad_string_dirty.find(">")])) static_directory = '/static' out_dir = os.getcwd() + static_directory in_memory = io.BytesIO() z = zipfile.ZipFile(in_memory, 'w', zipfile.ZIP_DEFLATED) z.writestr('pipes.scad', scad_string) z.write(os.path.join(out_dir, "prism.scad"), "prism.scad") z.close() in_memory.seek(0) return (in_memory)
def computeAnglesFromSizes(a, b, c): a1,b1,c1,A1,B1,C1 = solve(a=a, b=b, c=c) rospy.loginfo("#A1,B1,C1 = " + str(A1/degree), + str(B1/degree), + str(C1/degree))
def _calculateRadiusAtHeight(self, height, station_elevation): """This function calculates the radius at the specified height above sealevel. This function takes into consideration both the height of the radar station and the ange of the radar beam. This is a rough estimate and does not take into account refraction or beam width. It also assumes a spherical earth. height: height above sea level in meters station_elevation: radar site height above sea level in meters returns: ground distance radius of radar in meters or None if height is not available """ # anything bigger can throw errors and this is outside of the operational specs of # the current and future VCPs - https://en.wikipedia.org/wiki/NEXRAD if height > 90000: return None # There should probably be a lower bound here but I haven't been able to find it yet #if height < ?: return None # highest point above sea level if station_elevation > 6267: return None # radar must be lower than specified height if station_elevation >= height: return None # based on figure 2 of http://www.radartutorial.eu/01.basics/Calculation%20of%20height.en.html # TODO: include refraction in the calculation height_km = height / 1000.0 - station_elevation / 1000 localized_radius = EARTH_RADIUS_KM + station_elevation / 1000 # Create an triange on a circle (2d earth), the three side are the length of # localized radius connected, the radar beam and the height + localized_radius # connecting to the beam end and the earth center height_of_beam_end = localized_radius + height_km # lowest angle at the radar site # 90 degrees + radar beam angle radar_site_angle = math.radians(90) + WSR88D_LOW_ANGLE ####### # Calculate max ground distance at 0.5 degrees for WSR88D_MAX_RADIUS constant #a, b, beam_distance, beam_point_angle, B, earth_center_angle = trianglesolver.solve( # a = EARTH_RADIUS_KM, c = WSR88D_BEAM_DISTANCE, B = radar_site_angle, ssa_flag='acute') #print earth_center_angle * EARTH_RADIUS_KM # WSR88D_MAX_RADIUS=229819.074224 ####### a, b, beam_distance, beam_point_angle, B, earth_center_angle = trianglesolver.solve( a=localized_radius, b=height_of_beam_end, B=radar_site_angle, ssa_flag='acute') # if our beam_distance is too high then check the highest beam if (beam_distance > WSR88D_BEAM_DISTANCE): # angle at the radar site # 90 degrees + radar beam angle radar_site_angle = math.radians(90) + WSR88D_HIGH_ANGLE a, b, beam_distance, beam_point_angle, B, earth_center_angle = trianglesolver.solve( a=localized_radius, b=height_of_beam_end, B=radar_site_angle, ssa_flag='acute') # if we still don't have a reasonable beam distance, return None if (beam_distance > WSR88D_BEAM_DISTANCE): return None # If beam distance is fine, solve for this height at max beam_distance so we can # get the appropriate ground distance for the radar's radius at this height a, b, c, A, radar_site_angle, earth_center_angle = trianglesolver.solve( a=localized_radius, b=height_of_beam_end, c=WSR88D_BEAM_DISTANCE) #print math.degrees(radar_site_angle) # the ground distance is the circular segment with angle earth_center_angle and r of localized_radius # this assumes a smooth earth (and a shperical cow) ground_distance_km = earth_center_angle * localized_radius ground_distance = ground_distance_km * 1000 return ground_distance
endeffector_z = 0.4 endeffector_x = 0.15 link_1 = 0.5 link_2 = 0.5 joint_2 = 0 side_c1 = 0 angle_a1 = 0 angle_a2 = 0 angle_b1 = 0 angle_b2 = 0 if endeffector_x > 0: a,b,c,A,B,C = solve(a=endeffector_z, b=endeffector_x, C=90*degree) print("Side C", c) print("Angle A: ", round(A / degree, 2)) print("Angle B: ", round(B / degree, 2)) print("Angle C: ", round(C / degree, 2)) side_c1 = c angle_a1 = A / degree angle_b1 = B / degree if side_c1 > (link_1 + link_2): print("Target out of reach!") else: a,b,c,A,B,C = solve(a=link_1, b=link_2, c=side_c1) print("Angle A opposide Link 1: ", round(A / degree, 2)) print("Angle B opposide Link_2: ", round(B / degree, 2))
def angle(a, b, c): degree = pi / 180 a, b, c, A, B, C = solve(a, b, c) Angle_A = A / degree y = int(math.floor(Angle_A)) return y
def distance(a: float, b: float, gamma: float): C = gamma * triangle.degree result = triangle.solve(b=b, a=a, C=C) return result[2]
`pip3 install trianglesolver` Når denne er installeret, skal den blot importeres, hvorefter alle funktioner frit kan bruges. Importeringen foregår ligesom med `math` biblioteket. Man ønsker dog normalt ikke at skulle skrive `trianglesolver` hver gang man skal benytte sig af biblioteket. Derfor kan man importere det og give det et kaldenavn, som forkortelse. Python ved så herefter at hver gang man skriver dette kaldenavn, så referere man til `trianglesolver`. For at importere med et kaldenavn, gør man følgende `import trianglesolver as ts` import trianglesolver as ts `trianglesolver` er nu importeret og alle funktioner er tilgængelige. Når man skal bruge biblioteket, skal man blot referere til det som `ts`. Hvis man vil løse en trekant, skal man som minimum kende tre karakteristiske dele omkring trekanten. Det kan enten være siderne, vinklerne eller nogen af siderne og nogle af vinklerne. Trekanten ABC, har siderne `a=3` og `b=4` og vinkel `C=25 grader` og man skal finde siden c. Denne kan man nu løse med `trianglesolver`. Først skal alle siderne i trekanten defineres, dette kan heldigvis gøres på en enkelt linje. Disse variable skal sættes lig med noget. Altså de skal indeholde et eller andet. I dette tilfælde, skal de indeholde løsningen. For at definere hvordan `trianglesolver` skal finde løsningen, skal man bruge den indbyggede funktion `solve()`. I `solve()` skal man angive de kendte værdier og hvad de er lig med. Se eksemplet nedenfor a, b, c, A, B, C = ts.solve(a=3, b=4, C=25*ts.degree) Nu er løsningen altså gemt i de 6 variable `a, b, c, A, B, C`. Derved her vi nu alle siderne gemt i `a, b, c` og vinklerne gemt i `A, B, C`. Grunden til at man ganger C værdien med `ts.degree` er, at `trianglesolver` forventer en værdi i radianer, men da opgavens værdi var i grader, skal denne altså konverteres til radianer. Man kan nu få Python til at vise resultatet for siden c. Dette gøres ved blot at skrive c i en celle. c Side c har altså længden 1,8. Det er vigtigt at huske at Python angiver decimal tal med et `.` og __IKKE__ et `,`. Man kan også få angivet de ukendte vinkler `A` og `B`. Dette gøres på samme måde som ved siden `c`. Dog er resultatet af `A og B` lige nu angivet i radianer, så dette skal omskrives til grader. Dette gøres ved blot at dividere med den samme konstant `ts.degree` som der blev multipliceret med tidligere. A / ts.degree B / ts.degree
def Triangle(): Variable1 = () Variable2 = () Variable3 = () a = str() b = str() c = str() A = str() B = str() C = str() print("Enter three values including at least one side") a = (input("""side a: """)) if len(a) != 0: Variable1 = a b = (input("""side b: """)) if len(b) != 0: if len(a) >= 1: Variable2 = b else: Variable1 = b c = (input("""side c: """)) if len(c) != 0: if len(Variable1) >= 1: if len(Variable2) >= 1: Variable3 = c else: Variable2 = c if len(Variable1) >= 1: Variable2 = c else: Variable1 = c if len(Variable1) >= 1 and len(Variable2) >= 1 and len(Variable3) >= 1: pass else: A = ((input("""angle A: """))) if len(A) != 0: if len(Variable1) >= 1 and len(Variable2) >= 1: Variable3 = A elif len(Variable1) >= 1: Variable2 = A B = (input("""angle B: """)) if len(B) != 0: if len(Variable1) >= 1 and len(Variable2) >= 1: Variable3 = B elif len(Variable1) >= 1: Variable2 = B C = (input("""angle C: """)) if len(C) != 0: if len(Variable1) >= 1 and len(Variable2) >= 1: Variable3 = C elif len(Variable1) >= 1: Variable2 = C # Filtered Variables Are Strings After The .join Function Sides = [a, b, c] Sidesfilter = filter(None, Sides) if len(Sides) == 1: FilteredSidesList = (''.join(Sidesfilter)) else: FilteredSidesList = (' '.join(Sidesfilter)) ItemsInSidesList = len(FilteredSidesList.split()) Angles = [A, B, C] AnglesFilter = filter(None, Angles) if len(Angles) == 1: FilteredAnglesList = (''.join(AnglesFilter)) else: FilteredAnglesList = (' '.join(AnglesFilter)) ItemsInAnglesList = len(FilteredAnglesList.split()) # if there are 3 sides if ItemsInSidesList == 3: Side1, Side2, Side3 = (FilteredSidesList.split()) a, b, c, A, B, C = solve(a=float(Side1), b=float(Side2), c=float(Side3)) # if theres 2 sides and 1 angle if ItemsInSidesList == 2: Side1, Side2 = (FilteredSidesList.split()) Angle3 = (FilteredAnglesList.split()) Side1 = str(Side1) Side2 = str(Side2) Angle3 = str(Angle3) Angle3 = Angle3.replace('[', '') Angle3 = Angle3.replace(']', '') Angle3 = Angle3.replace("'", '') if Variable1 == a and Variable2 == b and Variable3 == A: a, b, c, A, B, C = solve(a=float(Side1), b=float(Side2), A=float(Angle3) * degree) elif Variable1 == b and Variable3 == c and Variable3 == A: a, b, c, A, B, C = solve(b=float(Side1), c=float(Side2), A=float(Angle3) * degree) elif Variable1 == a and Variable2 == c and Variable3 == A: a, b, c, A, B, C = solve(b=float(Side1), c=float(Side2), A=float(Angle3) * degree) elif Variable1 == a and Variable2 == b and Variable3 == B: a, b, c, A, B, C = solve(a=float(Side1), b=float(Side2), B=float(Angle3) * degree) elif Variable1 == b and Variable3 == c and Variable3 == B: a, b, c, A, B, C = solve(b=float(Side1), c=float(Side2), B=float(Angle3) * degree) elif Variable1 == a and Variable3 == c and Variable3 == B: a, b, c, A, B, C = solve(a=float(Side1), c=float(Side2), B=float(Angle3) * degree) elif Variable1 == a and Variable2 == b and Variable3 == C: a, b, c, A, B, C = solve(a=float(Side1), b=float(Side2), C=float(Angle3) * degree) elif Variable1 == b and Variable3 == c and Variable3 == C: a, b, c, A, B, C = solve(b=float(Side1), c=float(Side2), C=float(Angle3) * degree) elif Variable1 == a and Variable3 == c and Variable3 == C: a, b, c, A, B, C = solve(a=float(Side1), c=float(Side2), C=float(Angle3) * degree) #if theres 1 side and 2 angles if ItemsInSidesList == 1: Side1 = (FilteredSidesList.split()) Angle2, Angle3 = (FilteredAnglesList.split()) Side1 = str(Side1) Angle2 = str(Angle2) Angle3 = str(Angle3) Angle3 = Angle3.replace('[', '') Angle3 = Angle3.replace(']', '') Angle3 = Angle3.replace("'", '') Angle2 = Angle2.replace('[', '') Angle2 = Angle2.replace(']', '') Angle2 = Angle2.replace("'", '') Side1 = Side1.replace('[', '') Side1 = Side1.replace(']', '') Side1 = Side1.replace("'", '') if Variable1 == a and Variable2 == A and Variable3 == B: a, b, c, A, B, C = solve(a=float(Side1), A=float(float(Angle2) * degree), B=float(Angle3) * degree) elif Variable1 == b and Variable2 == A and Variable3 == B: a, b, c, A, B, C = solve(b=float(Side1), A=float(float(Angle2) * degree), B=float(Angle3) * degree) elif Variable1 == c and Variable2 == A and Variable3 == B: a, b, c, A, B, C = solve(c=float(Side1), A=float(float(Angle2) * degree), B=float(Angle3) * degree) elif Variable1 == a and Variable2 == B and Variable3 == C: a, b, c, A, B, C = solve(a=float(Side1), B=float(float(Angle2) * degree), C=float(Angle3) * degree) elif Variable1 == b and Variable2 == B and Variable3 == C: a, b, c, A, B, C = solve(b=float(Side1), B=float(float(Angle2) * degree), C=float(Angle3) * degree) elif Variable1 == c and Variable2 == B and Variable3 == C: a, b, c, A, B, C = solve(c=float(Side1), B=float(float(Angle2) * degree), C=float(Angle3) * degree) elif Variable1 == a and Variable2 == A and Variable3 == C: a, b, c, A, B, C = solve(a=float(Side1), A=float(float(Angle2) * degree), C=float(Angle3) * degree) elif Variable1 == b and Variable2 == A and Variable3 == C: a, b, c, A, B, C = solve(b=float(Side1), A=float(float(Angle2) * degree), C=float(Angle3) * degree) elif Variable1 == c and Variable2 == A and Variable3 == C: a, b, c, A, B, C = solve(c=float(Side1), A=float(float(Angle2) * degree), C=float(Angle3) * degree) A = (float(A / degree)) B = (float(float(B) / degree)) C = (float(C / degree)) if round(A) == round(B) or round(B) == round(C) or round(A) == round(C): a = round(float(a), 2) b = round(float(b), 2) c = round(float(c), 2) A = round(float(A)) B = round(float(B)) C = round(float(C)) type = "Isosceles" if round(A) == 60 and round(B) == 60 and round(C) == 60: a = round(float(a), 2) b = round(float(b), 2) c = round(float(c), 2) A = 60 B = 60 C = 60 type = "equilateral" elif round(A) == 90 or round(B) == 90 or round(C) == 90: a = round(float(a), 2) b = round(float(b), 2) c = round(float(c), 2) A = round(float(A), 2) B = round(float(B), 2) C = round(float(C), 2) type = "Right" else: a = round(float(a), 2) b = round(float(b), 2) c = round(float(c), 2) A = round(float(A), 2) B = round(float(B), 2) C = round(float(C), 2) type = "Scalene" print("Side a is " + str(a)) print("Side b is " + str(b)) print("Side c is " + str(c)) print("Angle A is " + str(A)) print("Angle B is " + str(B)) print("Angle C is " + str(C)) if type == "Right": print("This triangle is a Right triangle") board = Turtle() if a < 50 or b < 50 or c < 50: multiple = 100 else: multiple = 10 board.forward(c * multiple) # draw base board.left(180 - B) board.forward(a * multiple) board.left(180 - C) board.forward(b * multiple) tkinter.mainloop()
def rateImage(image_path, depth_tumors_path, depth_liver_path, num_tumors, verbose=False, plot=False, images=None): """ rates a given image by summing up tumor distances and comparing to enclosing circle of liver """ if not images: img = cv2.imread(image_path) # get depth images depth_tumors = exrToNumpy(depth_tumors_path) depth_liver = exrToNumpy(depth_liver_path) else: img = images[0] depth_tumors = images[1] depth_liver = images[2] num_tumors = num_tumors b, g, r = cv2.split(img) # threholding, closing of tumors/liver ret, thresh1 = cv2.threshold(b, 75, 255, cv2.THRESH_BINARY) ret, thresh2 = cv2.threshold(r, 75, 277, cv2.THRESH_BINARY) kernel = np.ones((5, 5), np.uint8) dilation_kernel = np.ones((2,2), np.uint8) closing = cv2.morphologyEx(thresh1, cv2.MORPH_CLOSE, kernel) closing_liver = cv2.morphologyEx(thresh2, cv2.MORPH_CLOSE, kernel) # dilation to make really small tumors bigger, because otherwise opencv will not recognize them, even if they are visible closing = cv2.dilate(closing, dilation_kernel, iterations=1) if plot: plt.subplot(151), plt.imshow(cv2.cvtColor(closing, cv2.COLOR_GRAY2BGR)), plt.title("closing") dst = cv2.addWeighted(img, 0.7, cv2.cvtColor(closing, cv2.COLOR_GRAY2BGR), 0.3, 0.0) _, contours_tumors, _ = cv2.findContours(closing.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) _, contours_liver, _ = cv2.findContours(closing_liver.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) cv2.drawContours(dst, contours_tumors, -1, (255, 0, 0), 1) # sum up liver contours and tumor area area_liver = cv2.contourArea(contours_liver[0]) liverPts = contours_liver[0] for contour in contours_liver[1:]: area_liver += cv2.contourArea(contour) liverPts = np.concatenate((liverPts, contour)) area_tumors = 0 for contour in contours_tumors: area_tumors += cv2.contourArea(contour) # calculate tumor momentums -> centers centers = [] for i in range(len(contours_tumors)): moments = cv2.moments(contours_tumors[i]) # check m00 because division by zero ??? profit has_zero_moments = moments['m00'] == 0.0 if verbose: print(moments['m00']) if not has_zero_moments: centers.append((moments['m10'] / moments['m00'], moments['m01'] / moments['m00'])) else: pass # print "a tumor has zero momentum" # check if at least 2 tumors are visible -> else no distance possible! if len(centers) > 1: pts = np.array(centers, np.int32) pts = pts.reshape((-1, 1, 2)) # calculate hull of tumor points hull = cv2.convexHull(pts) cv2.drawContours(dst, [hull], 0, (255, 255, 255)) # calculate enclosing circle of liver (x, y), radius = cv2.minEnclosingCircle(liverPts) center = (int(x), int(y)) # center = (int(y), int(x)) radius = int(0.60 * radius) cv2.circle(dst, center, radius, (255, 255, 255), 1) # calculate distances of tumors (arclength) arcLength = cv2.arcLength(hull, True) # calculate max score possible distance angle = 360.0 / num_tumors * degree if verbose: print("calculating maxDistance: a={0}, b={1}, C={2}".format(radius, radius, angle)) _, _, maxDistance, _, _, _ = solve(a=radius, b=radius, C=angle) score = arcLength / (maxDistance * num_tumors) # punish views with less tumors punish_factor = len(centers) / float(num_tumors) if len(centers) <= num_tumors else 1 # score = score * punish_factor if score > 1: score = 1 # grab depth values of centers dist_avg = 0 for (x, y) in centers: x = int(x) y = int(y) dt = getSmallestDepthInSurrounding(depth_tumors, x, y) dl = getSmallestDepthInSurrounding(depth_liver, x, y) dist = dt - dl dist_avg += dist if verbose: print("distance tumor to liver at ({0}, {1}): {2}".format(x, y, dist)) print( "exact values liver: {0} tumors: {1}".format(depth_liver[y, x], depth_tumors[y, x])) dist_avg /= len(centers) if verbose: print("average tumor to liver distance:", dist_avg) # distance rating maxDistance = 100 dist_margin = 10 if dist_avg <= dist_margin: dist_score = 1 else: # solve equation for score function a = np.array([[dist_margin, 1], [100, 1]]) b = np.array([1, 0]) solution = np.linalg.solve(a, b) m = solution[0] b = solution[1] dist_score = m * dist_avg + b # dist_score = 1.0 - ((dist_avg)/ maxDistance) if dist_score > 1: dist_score = 1 elif dist_score < 0: dist_score = 0 else: arcLength = 0 score = 0 dist_score = 0 punish_factor = 0 score_final = math.pow(0.5 * score + 0.5 * dist_score, 2) * punish_factor if verbose: # print "len(contours_liver)", len(contours_liver) print("#####\n\n") print("dist_score", dist_score, "score", score) print("final_score: ", score_final) print("arcLength: ", arcLength) print("Tumorezentren:", centers) print("sichtbare Tumore:", len(contours_tumors)) print("Leberflaeche: ", area_liver) print("Tumorflaeche: ", area_tumors) if plot: plt.subplot(152), plt.imshow(cv2.cvtColor(dst, cv2.COLOR_BGR2RGB)), plt.title("dst") plt.subplot(153), plt.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB)), plt.title("img") dt = exrToNumpy(depth_tumors_path) dl = exrToNumpy(depth_liver_path) plt.subplot(154), plt.imshow(dt), plt.title("dt") plt.subplot(155), plt.imshow(dl), plt.title("dl") plt.show() return score_final, len(centers)