def meridian2(P0,Rn,sign,q0):
print 'meridian2'
Q=[q0]
dt = 1e-4
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 = coating.optmizeTan(y, pnew,tol)
if dot(res.x-y,res.x-y)==0:tol*=0.1
y=res.x
if notstop:
norm = polynomial_spline.dfn(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 = polynomial_spline.dfn(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 FindNextParallel(P0, sign):
dt = 1e-4
y = array([float(P0[0]), float(P0[1]), float(P0[2])])
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
dif = d - Rn
S = (dif > 0)
tol = 1e-6
while abs(dif) > 1e-4:
tand = tangentd(y, sign) * dt
pnew = y + tand
res = polynomial_spline.optmizeTan(y, pnew, tol)
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)
norm = polynomial_spline.dfn(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 Qvector(y, Q, sign):
global handles
suc = True
while suc:
tan, q, suc = tangentOptm(y[-1], Q[-1])
if not coating.CheckDOFLimits(robot, q):
#print('deu bode')
print polynomial_spline.v
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)
xold = y[-1][0:3]
pnew = xold + sign * tan * dt
tol = 1e-5
xnew = xold
while dot(xnew - xold, xnew - xold) <= tol:
res = coating.optmizeTan(xold, pnew, tol)
tol *= 0.1
xnew = res.x
temp = polynomial_spline.fn(xnew[0], xnew[1], xnew[2])
dv = array(polynomial_spline.dfn(xnew[0], xnew[1], xnew[2]))
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((xnew, n))
y.append(P)
handles = plotPoint(P, handles, array((0, 1, 0)))
return Q, y
def FindNextParallel(P0,sign):
dt = 1e-4
y=array([float(P0[0]),float(P0[1]),float(P0[2])])
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
dif = d-Rn
S=(dif>0)
tol=1e-6
while abs(dif)>1e-4:
tand = tangentd(y,sign)*dt
pnew = y+tand
res = polynomial_spline.optmizeTan(y, pnew, tol)
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)
norm = polynomial_spline.dfn(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 Qvector(y,Q,sign):
global handles
suc=True
while suc:
tan, q, suc = tangentOptm(y[-1],Q[-1])
if not coating.CheckDOFLimits(robot,q):
#print('deu bode')
print polynomial_spline.v
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)
xold = y[-1][0:3]
pnew = xold + sign*tan*dt
tol=1e-5
xnew = xold
while dot(xnew-xold,xnew-xold)<=tol:
res = coating.optmizeTan(xold, pnew, tol)
tol*=0.1
xnew = res.x
temp = polynomial_spline.fn(xnew[0], xnew[1], xnew[2])
dv = array(polynomial_spline.dfn(xnew[0],xnew[1],xnew[2]))
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((xnew,n))
y.append(P)
handles=plotPoint(P, handles,array((0,1,0)))
return Q,y
def meridian2(P0, Rn, sign, q0):
print 'meridian2'
Q = [q0]
dt = 1e-4
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 = coating.optmizeTan(y, pnew, tol)
if dot(res.x - y, res.x - y) == 0: tol *= 0.1
y = res.x
if notstop:
norm = polynomial_spline.dfn(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 = polynomial_spline.dfn(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,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)
tan *= sign*dt
pnew = y+tan
res = coating.optmizeTan(y, pnew, tol)
if dot(res.x-y,res.x-y)==0:
tol*=0.1
y=res.x
norm = polynomial_spline.dfn(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 tangent(ray,sign):
P=ray
x=P[0];y=P[1];z=P[2]
dv = array(polynomial_spline.dfn(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 tangent(ray, sign):
P = ray
x = P[0]
y = P[1]
z = P[2]
dv = array(polynomial_spline.dfn(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 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)
tan *= sign * dt
pnew = y + tan
res = coating.optmizeTan(y, pnew, tol)
if dot(res.x - y, res.x - y) == 0:
tol *= 0.1
y = res.x
norm = polynomial_spline.dfn(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 consfunc_deriv(P):
#return dpolynomial(P,rR)
return polynomial_spline.dfn(P[0], P[1], P[2])
