Exemplo n.º 1
0
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
Exemplo n.º 2
0
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
Exemplo n.º 3
0
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
Exemplo n.º 4
0
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
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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
Exemplo n.º 7
0
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
Exemplo n.º 8
0
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
Exemplo n.º 9
0
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
Exemplo n.º 10
0
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
Exemplo n.º 11
0
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
Exemplo n.º 12
0
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