def tangent(ray, sign):
x = ray[0]
y = ray[1]
z = ray[2]
dv = array(polynomial_spline.df([x, y, z]))
n = dv / sqrt(dot(dv, dv))
tan = cross(n, [x, y, z])
return tan / sqrt(dot(tan, tan))
def onecurvepoint(p0):
tol=1e-4
while True:
df = polynomial_spline.df(p0)
p1 = p0 - polynomial_spline.f(p0)*df/dot(df,df)
dif = p1-p0
if sqrt(dot(dif,dif))<tol:
return p1
else: p0=p1
def drawParallel(ray, q0, sign):
viable = isViable(q0, ray[3:6])
Ql = []
yL = []
Qr = []
yR = []
if viable:
print 'drawParallel: is viable'
QL = [q0]
QR = [q0]
y = [ray]
#Arriscado - Pode caminhar em duas funcoes esquerda-direita
if sign == 1:
Qr, yR = Qvector(y, QR, sign)
#print 'sign 1: Qr -',array(Qr).shape,', yR -',array(yR).shape
y = [ray]
sign *= -1
Ql, yL = Qvector(y, QL, sign)
#print 'sign -1: Ql -',array(Ql).shape,', yL -',array(yL).shape
else:
Ql, yL = Qvector(y, QL, sign)
y = [ray]
sign *= -1
Qr, yR = Qvector(y, QR, sign)
else:
sign *= -1
print 'drawParallel: solution not found'
tol = 1e-6
while not viable:
y = ray[0:3]
tan, q0, viable = tangentOptm(ray, q0)
y = curvepoint(y + sign * tan * dt)
norm = polynomial_spline.df([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]])
print 'drawParallel: solution found'
QL = [q0]
QR = [q0]
y = [ray]
#Arriscado - Pode caminhar em duas funcoes esquerda-direita
if sign == 1:
Qr, yR = Qvector(y, QR, sign)
else:
Ql, yL = Qvector(y, QL, sign)
#return drawParallel(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 onecurvepoint(p0):
tol = 1e-4
while True:
df = polynomial_spline.df(p0)
p1 = p0 - polynomial_spline.f(p0) * df / dot(df, df)
dif = p1 - p0
if sqrt(dot(dif, dif)) < tol:
return p1
else:
p0 = p1
def FindNextParallel(P0,sign):
global Rn
d = sqrt(P0[0]**2+P0[1]**2+P0[2]**2)
R0=1.425
n = math.ceil((d-R0)/0.003)
Rn = R0+n*0.003
y = curvepoint(P0)
norm = polynomial_spline.df([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 FindNextParallel(P0, sign):
global Rn
d = sqrt(P0[0]**2 + P0[1]**2 + P0[2]**2)
R0 = 1.425
n = math.ceil((d - R0) / 0.003)
Rn = R0 + n * 0.003
y = curvepoint(P0)
norm = polynomial_spline.df([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 tangentd(ray,sign):
P=ray
x=P[0];y=P[1];z=P[2]
dv = array(polynomial_spline.df([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))
tand = sign*cross(a,n)
return tand
def curvepoint(p0):
tol=1e-4
while True:
df1 = polynomial_spline.df(p0); f1 = polynomial_spline.f(p0)
df2 = array([2*p0[0],2*p0[1],2*p0[2]]); f2 = p0[0]**2+p0[1]**2+p0[2]**2-Rn**2
df1df2 = dot(df1,df2); df1df1 = dot(df1,df1); df2df2 = dot(df2,df2)
beta = (-f1+f2*df1df1/df1df2)/(df1df2-df1df1*df2df2/df1df2)
alpha = (-f1+f2*df1df2/df2df2)/(df1df1-df1df2*df1df2/df2df2)
dk = alpha*df1+beta*df2
p1 = p0+dk
dif = p1-p0
if sqrt(dot(dif,dif))<tol:
return p1
else: p0=p1
def drawParallel(ray,q0,sign):
viable=isViable(q0,ray[3:6])
Ql=[]; yL=[]; Qr=[]; yR=[]
if viable:
print 'drawParallel: is viable'
QL=[q0]
QR=[q0]
y=[ray]
#Arriscado - Pode caminhar em duas funcoes esquerda-direita
if sign==1:
Qr, yR = Qvector(y,QR,sign)
#print 'sign 1: Qr -',array(Qr).shape,', yR -',array(yR).shape
y=[ray]
sign*=-1
Ql, yL = Qvector(y,QL,sign)
#print 'sign -1: Ql -',array(Ql).shape,', yL -',array(yL).shape
else:
Ql, yL = Qvector(y,QL,sign)
y=[ray]
sign*=-1
Qr, yR = Qvector(y,QR,sign)
else:
sign*=-1
print 'drawParallel: solution not found'
tol = 1e-6
while not viable:
y=ray[0:3]
tan, q0, viable = tangentOptm(ray,q0)
y=curvepoint(y+sign*tan*dt)
norm = polynomial_spline.df([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]])
print 'drawParallel: solution found'
QL=[q0]
QR=[q0]
y=[ray]
#Arriscado - Pode caminhar em duas funcoes esquerda-direita
if sign==1:
Qr, yR = Qvector(y,QR,sign)
else:
Ql, yL = Qvector(y,QL,sign)
#return drawParallel(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 tangentd(ray, sign):
P = ray
x = P[0]
y = P[1]
z = P[2]
dv = array(polynomial_spline.df([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))
tand = sign * cross(a, n)
return tand
def meridian2(P0,sign,q0):
print 'meridian2'
Q=[q0]
P0=array([P0[0],P0[1],P0[2]])
y = curvepoint(P0)
norm = polynomial_spline.df(y)
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 meridian2(P0, sign, q0):
print 'meridian2'
Q = [q0]
P0 = array([P0[0], P0[1], P0[2]])
y = curvepoint(P0)
norm = polynomial_spline.df(y)
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 curvepoint(p0):
tol = 1e-4
while True:
df1 = polynomial_spline.df(p0)
f1 = polynomial_spline.f(p0)
df2 = array([2 * p0[0], 2 * p0[1], 2 * p0[2]])
f2 = p0[0]**2 + p0[1]**2 + p0[2]**2 - Rn**2
df1df2 = dot(df1, df2)
df1df1 = dot(df1, df1)
df2df2 = dot(df2, df2)
beta = (-f1 + f2 * df1df1 / df1df2) / (df1df2 -
df1df1 * df2df2 / df1df2)
alpha = (-f1 + f2 * df1df2 / df2df2) / (df1df1 -
df1df2 * df1df2 / df2df2)
dk = alpha * df1 + beta * df2
p1 = p0 + dk
dif = p1 - p0
if sqrt(dot(dif, dif)) < tol:
return p1
else:
p0 = p1
def Qvector(y,Q,sign):
global handles
suc=True
while suc:
tan, q, suc = tangentOptm(y[-1],Q[-1])
if not coating.CheckDOFLimits(robot,q):
wait = input("deu bode, PRESS 1 ENTER TO CONTINUE.")
iksolList = fullSolution(y[-1])
Q=[iksolList[0][0]]
Q = Qvector_backtrack(y,Q)
continue
if suc:
Q.append(q)
p1 = curvepoint(y[-1][0:3]+sign*tan*dt)
dv = array(polynomial_spline.df(p1))
normdv = sqrt(dot(dv,dv))
#print 'success:', res.success, ' fn:', polynomial_spline.fn4(xnew[0],xnew[1],xnew[2])/normdv
n = dv/normdv
P=concatenate((p1,n))
y.append(P)
handles=plotPoint(P, handles,array((0,1,0)))
return Q,y
def Qvector(y, Q, sign):
global handles
suc = True
while suc:
tan, q, suc = tangentOptm(y[-1], Q[-1])
if not coating.CheckDOFLimits(robot, q):
wait = input("deu bode, PRESS 1 ENTER TO CONTINUE.")
iksolList = fullSolution(y[-1])
Q = [iksolList[0][0]]
Q = Qvector_backtrack(y, Q)
continue
if suc:
Q.append(q)
p1 = curvepoint(y[-1][0:3] + sign * tan * dt)
dv = array(polynomial_spline.df(p1))
normdv = sqrt(dot(dv, dv))
#print 'success:', res.success, ' fn:', polynomial_spline.fn4(xnew[0],xnew[1],xnew[2])/normdv
n = dv / normdv
P = concatenate((p1, n))
y.append(P)
handles = plotPoint(P, handles, array((0, 1, 0)))
return Q, y
def tangent(ray,sign):
x=ray[0];y=ray[1];z=ray[2]
dv = array(polynomial_spline.df([x,y,z]))
n = dv/sqrt(dot(dv,dv))
tan = cross(n,[x,y,z])
return tan/sqrt(dot(tan,tan))
