ZRA – ex21_Multiturn.py

				
					#!/usr/bin/python
# -*- coding: utf-8 -*-
## 1. 초기 설정① #######################################
# 라이브러리 가져오기
## 1．초기 설정 ①　모듈 가져오기 ######################
from i611_MCS import *
from teachdata import *
from i611_extend import *
from rbsys import *
from i611_common import *
from i611_io import *
from i611shm import * 

def main():
    ## 2. 초기 설정② ####################################
    # ZERO 로봇 생성자
    rb = i611Robot()
    # 좌표계의 정의
    _BASE = Base()
    # 로봇과 연결 시작 초기화
    rb.open()
    # I/O 입출력 기능의 초기화 
    IOinit( rb )
    # 교시 데이터 파일 읽기
    data = Teachdata( "teach_data" )

    ## 1. 교시 포인트 설정 ######################
    J1  = Joint(105, 45, 60, 0, 75, 45)
    P1  = rb.Joint2Position(J1)
    P1A = P1.offset(dz=100)
    P2  = P1.offset(dx=-800)
    P2A = P2.offset(dz=100)

    # p2 = data.get_position( "pos12", 1 ) # 0 
    # p3 = data.get_position( "pos12", 2 ) # F
    # p4 = data.get_position( "pos12", 3 ) # 1

    ## 2. 동작 조건 설정 ######################## 
    m = MotionParam( jnt_speed=10, lin_speed=10, pose_speed=10, overlap = 30, ik_solver_option = 0x11111111 )
    #MotionParam 형으로 동작 조건 설정
    rb.motionparam( m )
    # pos01=rb.getpos()
    #rb.move(pos01.offset(dz=50))

    ## 3. 로봇 동작을 정의 ##############################
    # 작업 시작
    rb.home()
    rb.move(J1)
  
    while True:
        rb.sleep(1)
        rb.use_mt(False)
        rb.move(P1)
        rb.move(P1A)
        rb.move(P2A)
        print "cur multiturn:", rb.getpos().position(True)[8]
        rb.move(P2)
        rb.move(P1)

        print "reverse"
        rb.use_mt(True)
        rb.move(P1)
        rb.move(P1A)
        rb.move(P2A)
        print "cur multiturn:", rb.getpos().position(True)[8]
        rb.move(P2)
        rb.move(P1)

        # rb.use_mt(True)
        # print 'po1', rb.getpos().position(True)[8]
        # rb.move(p1)
        # rb.move(p2)
      
        # rb.move(p1)
        # print 'po2' 
        # rb.move(p3)

        # rb.move(p1)
        # print 'po3'  
        # rb.move(p4)

    ## 4. 종료 ######################################
    # 로봇과의 연결을 종료
    rb.close()
if __name__ == '__main__':
    main()