Beispiel #1
0
def GoToPos( x,y,z,gr ):
    ser = serial.Serial("/dev/ttyACM0", baudrate=115200, timeout=3.0)
    s = invKin(x,y,z,[111.628,70.866,-65.91])
    print(s)
    duty = goToDegree(s[0],s[1]+8,s[2],s[3],s[4],gr)
    print(duty)
    ser.write(str(0)+str(0)+str(1))
    time.sleep(0.1)
    ser.write(duty[0:9])
    time.sleep(0.01)
    ser.write(duty[9:18])
Beispiel #2
0
def GoToPos( x,y,z,gr ):
    #check the baud rate, change the com port depending on the com, linux has another format ex.: '/dev/tty.usbserial'
    ser = serial.Serial("/dev/ttyACM0", baudrate=115200, timeout=3.0)
    s = invKin(x,y,z,[111.628,70.866,-65.91])
    print(s)
    #if the upcoming line is not working; try this:ser.write(b'obj,'0'+'0'+str(1)')
    duty = goToDegree(s[0],s[1]+8,s[2],s[3],s[4],gr)
    #duty = '364379490123348508'
    print(duty)
    ser.write(str(0)+str(0)+str(1))
    time.sleep(0.1)
    #use hint in the above comment if needed
    ser.write(duty[0:9])
    time.sleep(0.01)
    #use hint in the above comment if needed
    ser.write(duty[9:18])
Beispiel #3
0
def DrawLineGoToPos(x0,y0,z0,x1,y1,z1,gr,initial_guess,divisions):
    x, y, z = symbols('x y z')
    ser = serial.Serial("/dev/ttyACM0", baudrate=115200, timeout=3.0)
    xflag = False
    yflag = False
    zflag = False

    if((x1-x0)==0):
        xflag = True

    if((y1-y0)==0):
        yflag = True

    if((z1-z0)==0):
        zflag = True

    if(not xflag):
        eqnX = (x-x0)/(x1-x0)

    if(not yflag):
        eqnY = (y-y0)/(y1-y0)

    if(not zflag):
        eqnZ = (z-z0)/(z1-z0)

    #check for the largest difference
    if(abs(x0-x1) >= abs(y0-y1)):
        if(abs(x0-x1) >= abs(z0-z1)):
            largest = 'x'
        else:
            largest = 'z'
    else:
        if(abs(y0-y1) >= abs(z0-z1)):
            largest = 'y'
        else:
            largest = 'z'

    #calculate step
    if(largest == 'x'):
        step = (x1-x0)/divisions
    elif(largest == 'y'):
        step = (y1-y0)/divisions
    else:
        step = (z1-z0)/divisions

    xVec = []
    yVec = []
    zVec = []

    for i in range (0,divisions+1):
        if(largest == 'x'):
            xVec.append(x0 + step*(i))
            xS = xVec[i]
            if(not yflag and not zflag):
                yVec.append(solve(eqnY-eqnX.subs(x,xS).evalf())[0])
                zVec.append(solve(eqnZ-eqnX.subs(x,xS).evalf())[0])
            elif(yflag and not zflag):
                yVec.append(y0)
                zVec.append(solve(eqnZ-eqnX.subs(x,xS).evalf())[0])
            elif(not yflag and zflag):
                zVec.append(z0)
                yVec.append(solve(eqnY-eqnX.subs(x,xS).evalf())[0])
            else:
                yVec.append(y0)
                zVec.append(z0)
        elif(largest == 'y'):
            yVec.append(y0 + step*(i))
            yS = yVec[i]
            if(not xflag and not zflag):
                xVec.append(solve(eqnX-eqnY.subs(y,yS).evalf())[0])
                zVec.append(solve(eqnZ-eqnY.subs(y,yS).evalf())[0])
            elif(xflag and not zflag):
                xVec.append(x0)
                zVec.append(solve(eqnZ-eqnY.subs(y,yS).evalf())[0])
            elif(not xflag and zflag):
                zVec.append(z0)
                xVec.append(solve(eqnX-eqnY.subs(y,yS).evalf())[0])
            else:
                xVec.append(x0)
                zVec.append(z0)
        else:
            zVec.append(z0 + step*(i))
            zS = zVec[i]
            if(not xflag and not yflag):
                xVec.append(solve(eqnX-eqnZ.subs(z,zS).evalf())[0])
                yVec.append(solve(eqnY-eqnZ.subs(z,zS).evalf())[0])
            elif(xflag and not yflag):
                xVec.append(x0)
                yVec.append(solve(eqnY-eqnZ.subs(z,zS).evalf())[0])
            elif(not xflag and yflag):
                yVec.append(y0)
                xVec.append(solve(eqnX-eqnZ.subs(z,zS).evalf())[0])
            else:
                xVec.append(x0)
                yVec.append(y0)
    print xVec
    print yVec
    print zVec

    s = invKin(xVec[0],yVec[0],zVec[0],initial_guess)
    th1 = [s[0]]
    th2 = [s[1]]
    th3 = [s[2]]
    th4 = [s[3]]
    th5 = [s[4]]

    for i in range (1,divisions+1):
        s = invKin(xVec[i],yVec[i],zVec[i],[th2[i-1],th3[i-1],th4[i-1]])
        th1.append(s[0])
        th2.append(s[1])
        th3.append(s[2])
        th4.append(s[3])
        th5.append(s[4])

    #send number of points
    if (divisions<10):
        ser.write(str(0)+str(0)+str(divisions+1)) 
    elif (divisions<100):
        ser.write(str(0)+str(divisions+1))
    else:
        ser.write(str(divisions+1))

    for i in range (0,divisions+1):
        duty = goToDegree(th1[i],th2[i]+8,th3[i],th4[i],th5[i],gr)
        print(duty)
        time.sleep(0.1)
        ser.write(duty[0:9])
        time.sleep(0.01)
        ser.write(duty[9:18])
    return {'angles':(goToDegree(th1[divisions],th2[divisions]+8,th3[divisions],th4[divisions],th5[divisions],gr)),'guess':[th2[divisions],th3[divisions],th4[divisions]]}

#print DrawLineGoToPos(0,21.22,12.86,-15,25,8.7,'open',[111.628,70.866,-65.91],10)
Beispiel #4
0
if 0:
    import sys
    from Htrans import htrans
    print(htrans(leg3.s))
    print(htrans(leg2.w))
    print(numpy.dot(numpy.linalg.inv(htrans(leg3.s)), htrans(leg2.w))) 
    sys.exit(0)

if 0:
    from L2l import L2l
    print("BEFORE")
    print("L1w", leg1.w)
    print("L1s", leg1.s)
    l1, l2, l3, L1, L2, L3, R = L2l(leg1, leg2, leg3)
    print("AFTER")
    print("L1w", leg1.w)
    print("L1s", leg1.s)
    sys.exit(0)
# ------------------------------------------------------------------------
#  ########## Computation of inverse Kinematics ###########################
#  ------------------------------------------------------------------------
from InvKin import invKin
[L1,L2,L3] = invKin(leg1, leg2, leg3, sample);

# ------------------------------------------------------------------------
#  ########## Computation of forward Kinematics ###########################
#  ------------------------------------------------------------------------
from FwdKin import fwdKin
[z,rx,ry, rz] = fwdKin(L1,L2,L3);

Beispiel #5
0
if 0:
    import sys
    from Htrans import htrans
    print(htrans(leg3.s))
    print(htrans(leg2.w))
    print(numpy.dot(numpy.linalg.inv(htrans(leg3.s)), htrans(leg2.w)))
    sys.exit(0)

if 0:
    from L2l import L2l
    print("BEFORE")
    print("L1w", leg1.w)
    print("L1s", leg1.s)
    l1, l2, l3, L1, L2, L3, R = L2l(leg1, leg2, leg3)
    print("AFTER")
    print("L1w", leg1.w)
    print("L1s", leg1.s)
    sys.exit(0)
# ------------------------------------------------------------------------
#  ########## Computation of inverse Kinematics ###########################
#  ------------------------------------------------------------------------
from InvKin import invKin
[L1, L2, L3] = invKin(leg1, leg2, leg3, sample)

# ------------------------------------------------------------------------
#  ########## Computation of forward Kinematics ###########################
#  ------------------------------------------------------------------------
from FwdKin import fwdKin
[z, rx, ry, rz] = fwdKin(L1, L2, L3)