def meridian(P0, Rn, sign):
dt = 1e-5
y = array([float(P0[0]), float(P0[1]), float(P0[2])])
d = sqrt(P0[0]**2 + P0[1]**2 + P0[2]**2)
dif = d - Rn
S = (dif > 0)
tol = 1e-6
t = time.time()
while abs(dif) > 1e-4:
t2 = time.time()
tand = tangentd(y, sign) * dt
print 'tangentd_time = ' + str(time.time() - t2)
pnew = y + tand
t2 = time.time()
res = moving_LS.optmizeTan(y, pnew, tol)
print 'optimizeTan_time = ' + str(time.time() - t2)
if dot(res.x - y, res.x - y) == 0: tol *= 0.1
y = res.x
d = sqrt(res.x[0]**2 + res.x[1]**2 + res.x[2]**2)
dif = d - Rn
if S != (dif > 0):
sign *= -1
dt *= 0.5
S = (dif > 0)
print 'while_time = ' + str(time.time() - t)
norm = moving_LS.dfn4(y[0], y[1], y[2])
norm /= sqrt(dot(norm, norm))
y = array([y[0], y[1], y[2], norm[0], norm[1], norm[2]])
return y
def drawParallel2(ray, q0, sign):
print 'drawparallel2'
viable = isViable(q0, ray[3:6])
dt = 1e-5
if viable:
QL = [q0]
QR = [q0]
y = [ray]
if sign == 1:
Qr, yR = Qvector(y, QR, dt, sign)
#print 'sign 1: Qr -',array(Qr).shape,', yR -',array(yR).shape
y = [ray]
sign *= -1
Ql, yL = Qvector(y, QL, dt, sign)
#print 'sign -1: Ql -',array(Ql).shape,', yL -',array(yL).shape
else:
Ql, yL = Qvector(y, QL, dt, sign)
y = [ray]
sign *= -1
Qr, yR = Qvector(y, QR, dt, sign)
else:
sign *= -1
print 'drawParallel2: solution not found'
tol = 1e-6
while not viable:
y = ray[0:3]
tan, q0, viable = tangentOptm(ray, q0)
tan *= sign * dt
pnew = y + tan
res = moving_LS.optmizeTan(y, pnew, v, tol)
if dot(res.x - y, res.x - y) == 0:
tol *= 0.1
y = res.x
norm = moving_LS.dfn4(y[0], y[1], y[2])
norm /= sqrt(dot(norm, norm))
ray = array([y[0], y[1], y[2], norm[0], norm[1], norm[2]])
return drawParallel2(ray, q0, sign)
print 'Ql -', array(list(reversed(Ql))).shape, ', Qr -', array(
Qr[1:]).shape
if Ql and Qr[1:]:
Q = concatenate((list(reversed(Ql)), Qr[1:]))
elif Ql:
Q = Ql
else:
Q = Qr
return yR, yL, Q
def drawParallel(ray, sign):
sol = solution(ray)
dt = 1e-5
if sol:
print len(sol[0])
q0 = sol[0][0]
QL = [q0]
QR = [q0]
y = [ray]
if sign == 1:
Qr, yR = Qvector(y, QR, dt, sign)
#print 'sign 1: Qr -',array(Qr).shape,', yR -',array(yR).shape
y = [ray]
sign *= -1
Ql, yL = Qvector(y, QL, dt, sign)
#print 'sign -1: Ql -',array(Ql).shape,', yL -',array(yL).shape
else:
Ql, yL = Qvector(y, QL, dt, sign)
y = [ray]
sign *= -1
Qr, yR = Qvector(y, QR, dt, sign)
else:
sign *= -1
print 'drawParallel: solution not found'
tol = 1e-6
while not solution(ray):
y = ray[0:3]
tan = tangent(y, sign) * dt
pnew = y + tan
res = moving_LS.optmizeTan(y, pnew, v, tol)
if dot(res.x - y, res.x - y) == 0: tol *= 0.1
y = res.x
norm = moving_LS.dfn4(y[0], y[1], y[2])
norm /= sqrt(dot(norm, norm))
ray = [y[0], y[1], y[2], norm[0], norm[1], norm[2]]
return drawParallel(ray, sign)
print 'Ql -', array(list(reversed(Ql))).shape, ', Qr -', array(
Qr[1:]).shape
if Ql and Qr[1:]:
Q = concatenate((list(reversed(Ql)), Qr[1:]))
elif Ql:
Q = Ql
else:
Q = Qr
return yR, yL, Q
def meridian2(P0, Rn, sign, q0):
print 'meridian2'
Q = [q0]
dt = 1e-5
y = array([float(P0[0]), float(P0[1]), float(P0[2])])
d = sqrt(P0[0]**2 + P0[1]**2 + P0[2]**2)
dif = d - Rn
S = (dif > 0)
tol = 1e-6
notstop = True
while abs(dif) > 1e-4:
tand = tangentd(y, sign) * dt
pnew = y + tand
res = moving_LS.optmizeTan(y, pnew, v, tol)
if dot(res.x - y, res.x - y) == 0:
tol *= 0.1
y = res.x
if notstop:
norm = moving_LS.dfn4(y[0], y[1], y[2])
norm /= sqrt(dot(norm, norm))
ray = array([y[0], y[1], y[2], norm[0], norm[1], norm[2]])
resQ = coating.optmizeQ(robot, ikmodel, manip, ray, Q[-1])
if resQ.success:
Q.append(resQ.x)
else:
print 'Meridian Error'
d = sqrt(res.x[0]**2 + res.x[1]**2 + res.x[2]**2)
dif = d - Rn
if S != (dif > 0):
notstop = False
sign *= -1
dt *= 0.5
S = (dif > 0)
norm = moving_LS.dfn4(y[0], y[1], y[2])
norm /= sqrt(dot(norm, norm))
ray = array([y[0], y[1], y[2], norm[0], norm[1], norm[2]])
resQ = coating.optmizeQ(robot, ikmodel, manip, ray, Q[-1])
if resQ.success:
Q.append(resQ.x)
else:
print 'Meridian Error'
return ray, Q
def Qvector(y, Q, dt, sign):
suc = True
while suc:
tan, q, suc = tangentOptm(y[-1], Q[-1])
#print 'Qvec: tan -',array(tan).shape,', Q -',array(Q).shape,', suc -',suc
if suc:
Q.append(q)
xold = y[-1][0:3]
pnew = xold + sign * tan * dt
tol = 1e-5
xnew = xold
while dot(xnew - xold, xnew - xold) == 0:
res = moving_LS.optmizeTan(xold, pnew, tol)
tol *= 0.1
xnew = res.x
dv = array(moving_LS.dfn4(xnew[0], xnew[1], xnew[2]))
n = dv / sqrt(dot(dv, dv))
P = concatenate((xnew, n))
y.append(P)
return Q, y