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) 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 = RBF.f([xnew[0], xnew[1], xnew[2]]) dv = array(RBF.df([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))) print RBF.f(P[0:3]) 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): #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 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 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 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 = RBF.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]]) 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 = RBF.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]]) 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 = RBF.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