[협동] ROS-2 프로그램을 활용한 협동로봇 동작 운영 실습 4일차

2024.12.30

 

4일차부터는 본격적으로 프로젝트를 시작하였다.

 

프로젝트의 목적은 다음과 같다.

 

11개의 컵을 쌓아놓은 후 로봇팔을 이용하여 하나씩 들어올려 총 4층의 탑을 쌓는 것이다.

 

1층에는 6개의 컵이 삼각형 모양으로 층을 이루고

 

2층에는 3개의 컵이 삼각형 모양으로 층을 이루고

 

3층에는 1개의 컵을 쌓는다.

 

4층에는 1개의 컵을 위아래를 뒤집어서 쌓는다.

 

 

import rclpy
import DR_init

# for single robot
ROBOT_ID = "dsr01"
ROBOT_MODEL = "m0609"
VELOCITY, ACC = 170, 170

DR_init.__dsr__id = ROBOT_ID
DR_init.__dsr__model = ROBOT_MODEL

OFF, ON = 0, 1

def main(args=None):
    rclpy.init(args=args)
    node = rclpy.create_node("force_control", namespace=ROBOT_ID)

    DR_init.__dsr__node = node

    try:
        from DSR_ROBOT2 import (
            release_compliance_ctrl,
            check_force_condition,
            task_compliance_ctrl,
            set_desired_force,
            set_tool,
            set_tcp,
            movej,
            movel,
            get_current_posx,
            set_digital_output,
            get_digital_input,
            wait,
            DR_FC_MOD_REL,
            DR_AXIS_Z,
            DR_BASE,
            DR_MV_MOD_REL,
        )

        from DR_common2 import posx

    except ImportError as e:
        print(f"Error importing DSR_ROBOT2 : {e}")
        return
    BASE_HEIGHT = 250
    #pathplanning    
    delta=[78,0,0,0,0,0] #첫번째 줄
    delta2=[38,68,0,0,0,0] #두번째 줄부터

    init_cuppos=[353.2068176269531, -50.46979904174805, 102.13847351074219,0,-180,0]  
    init_cupposN=[]
    init_cupposCopy=init_cuppos.copy() 
    path=[]
    i=3 #1층 1번째 줄 컵 갯수

    for r in range(i):

        init_cupposN.append(init_cupposCopy.copy())
        #1층
        Nfloor=[init_cupposN[r].copy()]
        for _ in range(i-1):
            init_cupposN[r][0]+=delta[0]
            Nfloor.append(init_cupposN[r].copy())
        k=i-1
        for j in range(i-1):    
            init_cupposN[r][1]+=delta2[1]
            if j%2==0:
                init_cupposN[r][0]-=delta2[0]
            else:
                init_cupposN[r][0]+=delta2[0]
            Nfloor.append(init_cupposN[r].copy())

            for _ in range(k-1):
                if j%2==0:
                    init_cupposN[r][0]-=delta[0]
                else:
                    init_cupposN[r][0]+=delta[0]
                Nfloor.append(init_cupposN[r].copy())
            k-=1
        i-=1
        init_cupposCopy[0]+=38
        init_cupposCopy[1]+=22
        init_cupposCopy[2]+=95
        path.append(Nfloor)
        # 계산된 PLACE 위치 리스트 가져오기
    print(path)
    placeList = path[0]+path[1]+path[2]

    def calculate_pick_positions(base_height, num_cups):
        """
        Generate pick positions based on stacked cup heights.
        처음 위치는 base_height에 위치하며, 이후 각 위치는 Z축으로 10씩 증가.
        """
        pick_positions = []
        for i in range(num_cups):
            if i == 0:
                # 첫 번째 위치는 base_height
                z = base_height
            else:
                # 이후 위치는 base_height에서 10씩 증가
                z = base_height - i * 11
            
            # 각 위치의 좌표 추가
            pick_positions.append([427.7593994140625, -184.2381591796875, z, 0,-180,0])
        
        return pick_positions

    def release():
        set_digital_output(2, ON)
        set_digital_output(1, OFF)
        print("Release object")
        wait(1)

    def grip():
        set_digital_output(1, ON)
        set_digital_output(2, OFF)
        print("Grip object")
        wait(1)

    def force_control_pick_and_place(pick_pos, place_pos):
        # 이동하여 물건 집기
        grip()
        pre_pick_pos = pick_pos.copy()  # 복사하여 원본 pick_pos 유지
        pre_pick_pos[2] += 8  # Z축으로 10 위로 이동
        pre_pick_pos1 = pre_pick_pos[2]
        if place_pos[2] > 180:
            pre_pick_pos1 = pre_pick_pos[2]
            pre_pick_pos[2] = 360
        movel(pre_pick_pos, vel=VELOCITY, acc=ACC)  # Z축 위로 이동
        movel([pre_pick_pos[0], pre_pick_pos[1], pre_pick_pos1, pre_pick_pos[3], pre_pick_pos[4], pre_pick_pos[5]], vel=VELOCITY, acc=ACC)
        # Force Control 활성화 (아래로 이동하며 감지)
        task_compliance_ctrl(stx=[500, 500, 500, 100, 100, 100])
        set_desired_force(fd=[0, 0, -10, 0, 0, 0], dir=[0, 0, 1, 0, 0, 0], mod=DR_FC_MOD_REL)

        while not check_force_condition(DR_AXIS_Z, max=5):  # Force 감지 대기
            pass
        height_pos = get_current_posx()[0][2]  # 감지된 Z축 위치 저장
        release_compliance_ctrl()
        pre_pick_pos[2] = height_pos
        movel([pre_pick_pos[0], pre_pick_pos[1], pre_pick_pos[2]+5, pre_pick_pos[3], pre_pick_pos[4], pre_pick_pos[5]], vel=VELOCITY, acc=ACC)  # Z축 위로 이동
        release()
        movel([pick_pos[0], pick_pos[1], height_pos-12, pick_pos[3], pick_pos[4], pick_pos[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE)
        grip()
        pre_pick_pos[2] += 150  # Z축으로 50 위로 이동
        if place_pos[2] > 180:
            pre_pick_pos[2] = 360
        movel(pre_pick_pos, vel=VELOCITY, acc=ACC)
        movel([place_pos[0], place_pos[1], pre_pick_pos[2], place_pos[3], place_pos[4], place_pos[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE)
        movel([place_pos[0], place_pos[1], place_pos[2]+10, place_pos[3], place_pos[4], place_pos[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE)
        #movel([place_pos[0], place_pos[1], place_pos[2]-10, place_pos[3], place_pos[4], place_pos[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE)
        task_compliance_ctrl(stx=[500, 500, 500, 100, 100, 100])
        set_desired_force(fd=[0, 0, -10, 0, 0, 0], dir=[0, 0, 1, 0, 0, 0], mod=DR_FC_MOD_REL)

        while not check_force_condition(DR_AXIS_Z, max=5):  # Force 감지 대기
            pass
        release_compliance_ctrl()
        release()
        pre_pick_pos[2] -= 150
        if place_pos[2]>193:
            place_pos[2]=336
        else:
            place_pos[2]=217
        movel([place_pos[0], place_pos[1], place_pos[2], place_pos[3], place_pos[4], place_pos[5]], vel=VELOCITY, acc=ACC)

    # PICK 위치 리스트
    pickList = calculate_pick_positions(BASE_HEIGHT, 11)  # 11개의 쌓인 컵 기준

    
    #초기 위치
    JReady = [367.471, 8.104, 194.494, 90.013, 179.963, 89.766]
    set_tool("Tool Weight_GR")
    set_tcp("GripperDA_v1")
    movel(JReady,vel=VELOCITY, acc=ACC)
    for i in range(len(placeList)):
        pick_pos = pickList[i % len(pickList)]  # pickList는 순환하여 사용
        place_pos = placeList[i]  # 순차적으로 placeList에서 위치 가져오기
        force_control_pick_and_place(pick_pos, place_pos)
    
    # ####
    movel([427,-220,80,90,90,90],vel=60, acc=60)
    movel([427,-177,80,90,90,90],vel=60, acc=60)
    grip()
    movel([427,-177,250,90,90,90],vel=60, acc=60)
    movej([0,0,0,0,0,180],vel=VELOCITY, acc=ACC,mod=DR_MV_MOD_REL)
    movel([427, -100, 290,60,90,-90],vel=60, acc=60)
    movel([427.80596923828125, -3.8458755016326904, 350,60,90,-90],vel=60, acc=60)
    task_compliance_ctrl(stx=[500, 500, 500, 100, 100, 100])
    set_desired_force(fd=[0, 0, -10, 0, 0, 0], dir=[0, 0, 1, 0, 0, 0], mod=DR_FC_MOD_REL)
    while not check_force_condition(DR_AXIS_Z, max=5):  # Force 감지 대기
        pass
    release_compliance_ctrl()
    release()
    movel([391.7555236816406, -63.046051025390625, 327.4129943847656, 53.128883361816406, 89.54312896728516, -89.16883087158203],vel=60, acc=60)
    movel([391.7555236816406, -63.046051025390625, 530, 53.128883361816406, 89.54312896728516, -89.16883087158203],vel=60, acc=60)
    movej([0,0,0,0,0,-180],vel=60, acc=60,mod=DR_MV_MOD_REL)
    movel([191.67477416992188, 0.5356343388557434, 270.2490234375, 123.4961166381836, 180.0, 126.570068359375], vel=60, acc=60)
    grip()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

 

 

실행 결과는 다음과 같다.

 

https://youtube.com/shorts/vXoJg2Z4Cto