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 = MLS_QP.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 = MLS_QP.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 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 = MLS_QP.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 = MLS_QP.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 nearInSurface(P,points):
_,idx = MLS_QP.Tree.query(P)
proximo=points[idx,0:3]
stemp=MLS_QP.s
while stemp>5:
MLS_QP.update_normal_const(stemp)
MLS_QP.s=stemp
proximo = min_distance.minPoint(proximo)
stemp*=0.9
print MLS_QP.s
return proximo
def nearInSurface(P, points):
_, idx = MLS_QP.Tree.query(P)
proximo = points[idx, 0:3]
stemp = MLS_QP.s
while stemp > 5:
MLS_QP.update_normal_const(stemp)
MLS_QP.s = stemp
proximo = min_distance.minPoint(proximo)
stemp *= 0.9
print MLS_QP.s
return proximo
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 = MLS_QP.optmizeTan(y, pnew, v,tol)
if dot(res.x-y,res.x-y)==0:
tol*=0.1
y=res.x
if notstop:
norm = MLS_QP.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 = MLS_QP.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 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 = MLS_QP.optmizeTan(y, pnew, v, tol)
if dot(res.x - y, res.x - y) == 0: tol *= 0.1
y = res.x
norm = MLS_QP.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 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 = MLS_QP.optmizeTan(y, pnew, v, tol)
if dot(res.x - y, res.x - y) == 0:
tol *= 0.1
y = res.x
norm = MLS_QP.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 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 = MLS_QP.optmizeTan(y, pnew, v, tol)
if dot(res.x - y, res.x - y) == 0:
tol *= 0.1
y = res.x
if notstop:
norm = MLS_QP.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 = MLS_QP.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 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 = MLS_QP.optmizeTan(y, pnew, v,tol)
if dot(res.x-y,res.x-y)==0:tol*=0.1
y=res.x
norm = MLS_QP.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 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 = MLS_QP.optmizeTan(y, pnew, v,tol)
if dot(res.x-y,res.x-y)==0:
tol*=0.1
y=res.x
norm = MLS_QP.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 tangent(ray,sign):
P=ray
x=P[0];y=P[1];z=P[2]
dv = array(MLS_QP.dfn4(x,y,z))
n = dv/sqrt(dot(dv,dv))
P=concatenate((P,n))
tan = cross(n,[x,y,z])
a = tan/sqrt(dot(tan,tan))
return a
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 = MLS_QP.optmizeTan(xold, pnew, tol)
tol*=0.1
xnew = res.x
dv = array(MLS_QP.dfn4(xnew[0],xnew[1],xnew[2]))
n = dv/sqrt(dot(dv,dv))
P=concatenate((xnew,n))
y.append(P)
return Q,y
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 = MLS_QP.optmizeTan(xold, pnew, tol)
tol *= 0.1
xnew = res.x
dv = array(MLS_QP.dfn4(xnew[0], xnew[1], xnew[2]))
n = dv / sqrt(dot(dv, dv))
P = concatenate((xnew, n))
y.append(P)
return Q, y
def tangent(ray, sign):
P = ray
x = P[0]
y = P[1]
z = P[2]
dv = array(MLS_QP.dfn4(x, y, z))
n = dv / sqrt(dot(dv, dv))
P = concatenate((P, n))
tan = cross(n, [x, y, z])
a = tan / sqrt(dot(tan, tan))
return a