ZRA – ex22_pass_through.py

				
					#!/usr/bin/python
#-*- coding: utf-8 -*-

from i611_MCS import *
from i611_extend import *
from i611_io import *
from i611shm import *
import sys
import time
import pdb
# from robotworld_greeting

# m = MotionParam(jnt_speed=20, lin_speed=100, acctime=0.2, dacctime=0.2, overlap=50)
def BackToHome(argRb, argHome):
    # argRb.motionparam(m)
    argRb.move(argHome)

def main():
    rb = i611Robot()
    _BASE = Base()
    rb.open()
    IOinit(rb)
    rb.enable_interrupt(1,True)

    jHome = Joint(0,0,0,0,0,0)
    j0 = Joint( j1=   0, j2 = 60, j3 = -90, j4 = 0, j5 = 40,  j6 = 0 )
    p0 = Position( x = 100, y =  250, z = 300, rx = -180, ry = 0, rz = 0, posture=6 )
    p1 = Position( x = 100, y = -250, z = 300, rx = -180, ry = 0, rz = 0, posture=7 )
    
    m = MotionParam(jnt_speed=100, lin_speed=1000, overlap=30, acctime=1, dacctime=1)
    rb.motionparam(m)

    rb.override(10)

    ################################    
    
    BackToHome(rb, jHome)
    try:
        while True:
           
            rb.move(p0)
            rb.relline(dz=-50)
            rb.move(p0)
            rb.move(p1)
            rb.relline(dz=-50)
            rb.move(p1)
   
    except KeyboardInterrupt, e:
        print "(except KeyboardInterrupt)", e
        BackToHome(rb, jHome)
    except Exception, e:
        print "(except Exception)", e
    BackToHome(rb, jHome)
    rb.close()

if __name__ == '__main__':
    main()