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)
