def computeNormal(L):
if vecAB(L) is None or vecAC(L) is None:
return None
vecNormal = np.cross(vecAB(L), vecAC(L))
norm = euclidienNorm(vecNormal)
return vecNormal / norm
def computeAngles(L):
if vecAB(L) is None or vecAC(L) is None:
return None
vecNormal = np.cross(vecAB(L), vecAC(L))
norm = euclidienNorm(vecNormal)
return [-90 + np.arccos(np.dot([1, 0, 0], vecNormal) / norm) * 180 / np.pi,
-90 + np.arccos(np.dot([0, 1, 0], vecNormal) / norm) * 180 / np.pi,
np.arccos(np.dot([0, 0, 1], vecNormal) / norm) * 180 / np.pi]
def computeTheta(L):
if vecAB(L) is None or vecAC(L) is None:
return None
vecNormal = np.cross(vecAB(L), vecAC(L))
normal = vecNormal / euclidienNorm(vecNormal)
nz = np.dot([0, 0, 1], normal)
return np.arccos(nz) * 180 / np.pi
def calculatePrecision3(x, y, z):
vectorFunction = VectorFunction([L1_, L2_, L3_])
x0 = [x, y, z]
matrix = np.linalg.inv(vectorFunction.jacobienMatrix(x0))
arrPrecision = []
for dl in gen_dL():
precision = euclidienNorm(np.dot(matrix, dl))
arrPrecision.append(precision)
return max(arrPrecision)
def computePhi(L):
if vecAB(L) is None or vecAC(L) is None:
return None
vecNormal = np.cross(vecAB(L), vecAC(L))
normal = vecNormal / euclidienNorm(vecNormal)
nx = np.dot([1, 0, 0], normal)
ny = np.dot([0, 1, 0], normal)
if nx == 0 and ny == 0:
return None
sign = np.sign(ny) if ny != 0 else 1
return np.arccos(nx / np.sqrt(nx ** 2 + ny ** 2)) * sign * 180 / np.pi
def findDistanceTranslation(vec, angles):
# params = [RA, RB, RC, LA, LB, LC]
# index = 0 1 2 3 4 5
def A(params):
return np.array([-params[0] * sinPi3, -params[0] * cosPi3, params[3]],
dtype=float)
def B(params):
return np.array([params[1] * sinPi3, -params[1] * cosPi3, params[4]],
dtype=float)
def C(params):
return np.array([0, params[2], params[5]], dtype=float)
n = normal(angles)
vec = np.array(vec, dtype=float)
arrCoordinateFunctions = [
lambda params: n.dot(A(params) - vec),
lambda params: n.dot(B(params) - vec),
lambda params: n.dot(C(params) - vec),
lambda params: euclidienNorm(B(params) - A(params)) - a,
lambda params: euclidienNorm(C(params) - A(params)) - a,
lambda params: euclidienNorm(C(params) - B(params)) - a
]
systemFunction = VectorFunction(coordinateFunctions=arrCoordinateFunctions)
startingPoint = np.array(
[R, R, R, L1(vec, angles),
L2(vec, angles),
L3(vec, angles)],
dtype=float)
solution = algorithmNewtonRaphson(systemFunction,
starting=startingPoint,
precision=10**-4)
return euclidienNorm(A(solution) - np.array([xA, yA, L1(vec, angles)], dtype=float)) * np.sign(R - d - solution[0]), \
euclidienNorm(B(solution) - np.array([xB, yB, L2(vec, angles)], dtype=float)) * np.sign(R - d - solution[1]), \
euclidienNorm(C(solution) - np.array([xC, yC, L3(vec, angles)], dtype=float)) * np.sign(R - d - solution[2])
def gamma(L):
vecNormal = np.cross(vecAB(L), vecAC(L))
norm = euclidienNorm(vecNormal)
return np.arccos(np.dot([0, 0, 1], vecNormal) / norm) * 180 / np.pi
def beta(L):
vecNormal = np.cross(vecAB(L), vecAC(L))
norm = euclidienNorm(vecNormal)
return -90 + np.arccos(np.dot([0, 1, 0], vecNormal) / norm) * 180 / np.pi
arrError = []
z = Z
for y in Y:
row = []
for x in X:
if not inWorkspace(x, y, z):
row.append(None)
else:
print([x, y])
desiredPoint = np.array([x, y, z], dtype=float)
arrLength = perfectDelta.inverseKinetic(x, y, z)
reachedPoint = imperfectDelta.forwardKinetic(*arrLength)
row.append(euclidienNorm(desiredPoint - reachedPoint))
#row.append(abs(desiredPoint[2] - reachedPoint[2]))
arrError.append(row)
fig = go.Figure(data=go.Contour(z=arrError, x=X, y=Y),
layout=(dict(height=600, width=600)))
fig.update_layout(title="Erreur quadratique en mm (pour z={0:.2f})".format(Z),
xaxis_title="X in mm",
yaxis_title="Y in mm",
font=dict(family="Courier New, monospace"))
fig.show()
# print(imperfectDelta.inverseKinetic(0, 0, 950))
