Recent Posts
printf("ho_tari\n");
[협동] ROS-2 프로그램을 활용한 협동로봇 동작 운영 실습 2일차 본문
2024.12.26
# pick and place in 1 method. from pos1 to pos2 @20241104
import rclpy
import DR_init
# for single robot
ROBOT_ID = "dsr01"
ROBOT_MODEL = "m0609"
VELOCITY, ACC = 60, 60
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,
)
from DR_common2 import posx
except ImportError as e:
print(f"Error importing DSR_ROBOT2 : {e}")
return
# 위치 리스트 (PICK & PLACE)
pickList = [[447.66845703125, 149.0836639404297, 240.62635803222656, 167.5069580078125, 180.0, 166.8032989501953],[400.4941711425781, 148.47911071777344, 188.19166564941406, 48.09524154663086, 180.0, 48.17793273925781],[346.8483581542969, 149.53216552734375, 193.3278350830078, 99.93858337402344, 180.0, 101.1941146850586]]
placeList = [[452.34429931640625, -0.11756063252687454, 190.96731567382812, 44.95091247558594, 180.0, 44.90541458129883],[396.9905700683594, -3.8059051036834717, 201.14144897460938, 20.58327293395996, 180.0, 21.770511627197266],[346.99237060546875, 0.9880767464637756, 218.13966369628906, 93.85528564453125, 180.0, 95.06006622314453]]
def release():
set_digital_output(2, ON)
set_digital_output(1, OFF)
print("release")
wait(1)
def grip():
#release()
set_digital_output(1, ON)
set_digital_output(2, OFF)
print("grip")
wait(1)
# 초기 위치
JReady = [226.76063537597656, 7.510124683380127, 210.95468139648438, 146.1909942626953, -179.83033752441406, 145.9329071044922]
set_tool("Tool Weight_2FG")
set_tcp("2FG_TCP")
# 초기 위치로 이동
movel([JReady[0], JReady[1], JReady[2], JReady[3], JReady[4], JReady[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE)
print("home")
previous_pos = 235
for pick_pos, place_pos in zip(pickList, placeList):
# Step 1: PICK 단계
release()
pick_pre = pick_pos.copy()
pick_pre[2] = previous_pos
movel(pick_pre, vel=VELOCITY, acc=ACC, ref=DR_BASE)
pick_z40 = pick_pos.copy()
pick_z40[2] = 70# Z축 40으로 설정
movel(pick_z40, vel=VELOCITY, acc=ACC, ref=DR_BASE) # pick z=40 위치로 이동
# 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()
# 위로 약간 상승
movel([pick_pos[0], pick_pos[1], height_pos + 10, pick_pos[3], pick_pos[4], pick_pos[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE)
grip() # 물체 잡기
movel([pick_pos[0], pick_pos[1], height_pos + 140, pick_pos[3], pick_pos[4], pick_pos[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE) # 위로 크게 이동
# Step 2: PLACE 단계
# Z축 높이를 유지한 상태에서 X, Y 이동
movel([place_pos[0], place_pos[1], height_pos + 140, pick_pos[3], pick_pos[4], pick_pos[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE)
movel([place_pos[0], place_pos[1], height_pos+50, pick_pos[3], pick_pos[4], pick_pos[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE)
# Force Control로 Z축 하강
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=8): # Force 감지 대기
pass
release() # 그리퍼 열기
release_compliance_ctrl()
# 위로 이동
movel([place_pos[0], place_pos[1], height_pos + 200, pick_pos[3], pick_pos[4], pick_pos[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE)
previous_pos = height_pos+200
# 최종 완료 후 초기 위치로 복귀
movel([JReady[0], JReady[1], JReady[2], JReady[3], JReady[4], JReady[5]], vel=VELOCITY, acc=ACC, ref=DR_BASE)
print("home")
rclpy.shutdown()
if __name__ == "__main__":
main()
이 코드는 금속블록의 위치 좌표로 로봇팔이 이동하도록 한 후 힘제어를 통해 금속블록을 들어올려 지정된 다른 위치 좌표로 이동시키는 내용이다. 실행 결과는 다음과 같다.
https://youtube.com/shorts/nj9h2WH4HF8
import rclpy
import DR_init
# ------------------------
# 로봇/툴/그리퍼 설정
# ------------------------
ROBOT_ID = "dsr01"
ROBOT_MODEL = "m0609"
VELOCITY, ACC = 60, 60 # 기본 속도/가속도
OFF, ON = 0, 1
# Doosan 로봇 init 설정
DR_init.__dsr__id = ROBOT_ID
DR_init.__dsr__model = ROBOT_MODEL
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node("force_pick_sort", 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,
wait,
DR_FC_MOD_REL,
DR_AXIS_Z,
DR_BASE,
)
from DR_common2 import posx
except ImportError as e:
print(f"Error importing DSR_ROBOT2 : {e}")
return
# --------------------------------------
# 1) 초기 준비: 툴/TCP/그리퍼/초기자세
# --------------------------------------
set_tool("Tool Weight_GR") # 예: 등록된 툴 명
set_tcp("GripperDA_v1") # 예: 등록된 그리퍼 TCP 명
# 초기 자세(예시): 실제 로봇 환경에서 안전 높이의 Pose
JReady = [226.7606, 7.5101, 210.9546, 146.1909, -179.8303, 145.9329]
def grip_open():
set_digital_output(2, ON)
set_digital_output(1, OFF)
print("release")
wait(1)
def grip_close():
#release()
set_digital_output(1, ON)
set_digital_output(2, OFF)
print("grip")
wait(1)
# 초기 위치로 이동 + 그리퍼는 닫힌 상태로 시작 (사용자가 요구한 시나리오)
movel(JReady, vel=VELOCITY, acc=ACC, ref=DR_BASE)
print("Move to Home.")
grip_close()
# --------------------------------------
# 2) Pick 후보 위치(여기서는 예시로 2~3개)
# 실제 물체 위치는 카메라 인식 or Teach 으로 지정
# --------------------------------------
pick_candidates = [
[450.33221435546875, 148.03155517578125, 240.62635803222656, 167.5069580078125, 180.0, 166.8032989501953],
[400.4941711425781, 148.47911071777344, 188.19166564941406, 48.09524154663086, 180.0, 48.17793273925781],
[348.27850341796875, 149.02813720703125, 193.3278350830078, 99.93858337402344, 180.0, 101.1941146850586],
]
# --------------------------------------
# 3) 높이에 따른 Place 위치 분류
# 예: contact_z < 100 → A, 100~150 → B, 150 이상 → C
# --------------------------------------
place_A = [449.85003662109375, -1.4901727437973022, 190.96731567382812, 44.95091247558594, 180.0, 44.90541458129883] # 낮은 물건
place_B = [396.9905700683594, -3.8059051036834717, 201.14144897460938, 20.58327293395996, 180.0, 21.770511627197266] # 중간
place_C = [347.75433349609375, -0.5798255801200867, 218.13966369628906, 93.85528564453125, 180.0, 95.06006622314453] # 높은 물건
# --------------------------------------
# 4) 메인 루프: Pick → 분류 → Place
# --------------------------------------
for idx, candidate_pos in enumerate(pick_candidates):
print(f"\n======= [Target {idx+1}] Start =======")
# 4-1) 물체 상부로 이동 (접근 위치)
movel(candidate_pos, vel=VELOCITY, acc=ACC, ref=DR_BASE)
print(f"Move to candidate_pos {candidate_pos}")
# ============================
# (A) 힘제어로 '접촉' 감지
# ============================
# 2) 일정 Z까지 movel (예: 120mm)
target_z = 100
force_down_pos = candidate_pos.copy()
force_down_pos[2] = target_z
movel(force_down_pos, vel=VELOCITY, acc=ACC, ref=DR_BASE)
# 1) 힘제어 활성화
task_compliance_ctrl(stx=[500, 500, 500, 100, 100, 100])
set_desired_force(fd=[0, 0, -10, 0, 0, 0], # Z- 방향 10N
dir=[0, 0, 1, 0, 0, 0],
mod=DR_FC_MOD_REL)
# 3) Force 감지 대기
while not check_force_condition(DR_AXIS_Z, max=5):
pass
print("[Force Detected] Contact found.")
# 4) 접촉 순간의 Z좌표 읽기
current_pose = get_current_posx()[0] # [x, y, z, rx, ry, rz]
contact_z = current_pose[2]
print(f" >> contact_z = {contact_z:.2f}")
# 5) 힘제어 해제
release_compliance_ctrl()
# ============================
# (B) 그리퍼 열고 조금 더 내려간 뒤 잡기
# ============================
# 1) 그리퍼 열기
grip_open()
# 2) 조금 더 하강 (ex: +10mm 더 내려가 보기)
# 실제 물건 형상에 따라 조정
pick_z = contact_z - 15
final_down_pos = current_pose.copy()
final_down_pos[2] = pick_z if pick_z > 0 else 0 # 음수로 내려가면 안 되니 체크
movel(final_down_pos, vel=60, acc=60, ref=DR_BASE)
print(f" >> Move down to {pick_z:.2f} and grip close")
# 3) 그리퍼 닫기 → 물건 잡기
grip_close()
# 4) 위로 상승 (예: contact_z + 50)
safe_up_pos = current_pose.copy()
safe_up_pos[2] = contact_z + 140
movel(safe_up_pos, vel=VELOCITY, acc=ACC, ref=DR_BASE)
print(" >> Move up with object")
# ============================
# (C) 높이별 분류
# ============================
# 예) contact_z < 100 → place_A
# 100 <= contact_z < 150 → place_B
# else → place_C
if contact_z < 61:
place_target = place_C
print(" >> Class: LOW object → Place A")
elif 65 <= contact_z < 71:
place_target = place_B
print(" >> Class: MEDIUM object → Place B")
else:
place_target = place_A
print(" >> Class: HIGH object → Place C")
# ============================
# (D) Place 지점 이동 → 내려놓기
# ============================
movel(place_target, vel=VELOCITY, acc=ACC, ref=DR_BASE)
print(f" >> Move to place target: {place_target}")
# 2) 힘제어 켜고, 하강
target_down = 100
place_down = place_target.copy()
place_down[2] = target_down # 100mm 더 내려가면서 바닥/물체 접촉 감지
movel(place_down, vel=VELOCITY, acc=ACC, ref=DR_BASE)
print(" >> Force control down to place...")
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)
# 3) Force 감지 대기
while not check_force_condition(DR_AXIS_Z, max=8):
pass
release_compliance_ctrl()
print("[Force Detected] Place contact found.")
# 4) 접촉 순간 그리퍼 오픈(물건 내려놓기)
grip_open()
# 힘제어 해제
# 5) 위로 상승
place_up = get_current_posx()[0] # 현재 접촉 지점 pose
place_up[2] += 100.0 # 50mm 위로
print("get_current_posx()[0]:", get_current_posx()[0])
print("place up:", place_up)
movel(place_up, vel=VELOCITY, acc=ACC, ref=DR_BASE)
print(" >> Object placed. Back up.")
# 다음 Pick 전, 그리퍼 닫거나 유지
grip_close()
# --------------------------------------
# 5) 모든 Pick & Place 완료 후 홈으로 복귀
# --------------------------------------
movel(JReady, vel=VELOCITY, acc=ACC, ref=DR_BASE)
print("Done. Back to Home.")
rclpy.shutdown()
if __name__ == "__main__":
main()
이 코드는 지정된 위치에 존재하는 금속블록의 높이를 힘제어를 통해 확인한 후 높이별로 구분하여 지정된 위치에 정렬하여 옮기는 내용이다. 실행 결과는 다음과 같다.
https://youtube.com/shorts/x2m_hnxs5Ds
# pick and place in 1 method. from pos1 to pos2 @20241104
import rclpy
import DR_init
import time
# for single robot
ROBOT_ID = "dsr01"
ROBOT_MODEL = "m0609"
VELOCITY, ACC = 200, 200
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("dsr_example_demo_py", namespace=ROBOT_ID)
DR_init.__dsr__node = node
try:
from DSR_ROBOT2 import (
set_digital_output,
get_digital_input,
release_compliance_ctrl,
check_force_condition,
task_compliance_ctrl,
set_desired_force,
get_current_posx,
set_tool,
set_tcp,
movej,
movel,
wait,
mwait,
DR_FC_MOD_REL,
DR_AXIS_Z,
DR_BASE,
trans,
)
from DR_common2 import posx, posj
except ImportError as e:
print(f"Error importing DSR_ROBOT2 : {e}")
return
JReady = posj([0, 0, 90, 0, 90, 0])
set_tool("Tool Weight_2FG")
set_tcp("2FG_TCP")
# 계산된 좌표 값
x00 = [498.748, 97.436, 23.648, 17.387, 179.305, 15.346]
x10 = [498.748, 44.982, 23.648, 17.387, 179.305, 15.346]
x20 = [498.748, -7.472, 23.648, 17.387, 179.305, 15.346]
x01 = [548.415, 97.436, 23.648, 17.387, 179.305, 15.346]
x11 = [548.415, 44.982, 23.648, 17.387, 179.305, 15.346]
x21 = [548.415, -7.472, 23.648, 17.387, 179.305, 15.346]
x02 = [598.082, 97.436, 23.648, 17.387, 179.305, 15.346]
x12 = [598.082, 44.982, 23.648, 17.387, 179.305, 15.346]
x22 = [598.082, -7.472, 23.648, 17.387, 179.305, 15.346]
k11 = [548.415, -103.545, 23.648, 17.387, 179.305, 15.346]
global pallet_i
pallet = [0,0,0,0,2,0,0,0,0]
top_pallet = [x00, x01, x02, x10, x11, x12, x20, x21, x22]
def wait_digital_input(sig_num):
# while not get_digital_input(sig_num):
# wait(0.5)
# print("Wait for digital input", sig_num)
# pass
wait(1)
def release():
set_digital_output(2, ON)
set_digital_output(1, OFF)
print("Release", end='')
wait_digital_input(2)
def grip():
release()
set_digital_output(1, ON)
set_digital_output(2, OFF)
# if flag:
# print("Grip", end='')
wait_digital_input(1)
wait(1.0)
def sort_block(z):
if z < 50:
if not 0 in pallet[0:3]:
return "s", "blank"
index = pallet.index(0,0,3)
print("S")
return "s",index
elif 50 <= z and z < 60:
if not 0 in pallet[3:6]:
return "m", "blank"
index = pallet.index(0,3,6)
print("M")
return "m", index
elif 60 <= z:
if not 0 in pallet[6:9]:
return "l", "blank"
index = pallet.index(0,6,9)
print("L")
return "l", index
else:
print("Error: height")
def check(base_pose):
base_point_up = trans(base_pose, [0,0,100,0,0,0], DR_BASE, DR_BASE )
force_start_point = trans(base_pose, [0,0,47,0,0,0], DR_BASE, DR_BASE)
movel(base_point_up, vel=VELOCITY, acc=ACC)
movel(force_start_point, vel=VELOCITY, acc=ACC)#내려감
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=10):
pass
release_compliance_ctrl()
time.sleep(2.0)
# z축 sorting
place = get_current_posx()
result = sort_block(place[0][2])
movel(base_point_up, vel=VELOCITY, acc=ACC)
return result
def pick(base_pose):
base_point_up = trans(base_pose, [0,0,100,0,0,0], DR_BASE, DR_BASE )
movel(base_point_up, vel=VELOCITY, acc=ACC)
release()
movel(base_pose, vel=VELOCITY, acc=ACC)
grip()
movel(base_point_up, vel=VELOCITY, acc=ACC)
print("current position4 : ", get_current_posx())
def place(base_pose):
base_pose_up = trans(base_pose, [0,0,70,0,0,0], DR_BASE, DR_BASE)
force_start_point = trans(base_pose, [0,0,27,0,0,0], DR_BASE, DR_BASE)
movel(base_pose_up, vel=VELOCITY, acc=ACC)
print("current position2 : ", get_current_posx())
movel(force_start_point, vel=VELOCITY, acc=ACC)#내려감
print("current position3 : ", get_current_posx())
task_compliance_ctrl(stx=[500, 500, 500, 100, 100, 100])
set_desired_force(fd=[0, 0, -15, 0, 0, 0], dir=[0, 0, 1, 0, 0, 0], time=0.1, mod=DR_FC_MOD_REL)
while not check_force_condition(DR_AXIS_Z, max=10):
pass
release_compliance_ctrl()
time.sleep(0.5)
release()
movel(base_pose_up, vel=VELOCITY, acc=ACC)
grip()
def handle_block_mismatch(block_size, assigned_index):
if assigned_index == "blank":
start = pallet.index(0)
end = pallet.index(2)
pick(top_pallet[start])
place(top_pallet[end])
pallet[start] = 2
pallet[end] = 1
return
block_size2, _ = check(top_pallet[assigned_index])
start = pallet.index(0)
end = assigned_index
buffer = pallet.index(2)
if block_size2 == block_size:
pallet[assigned_index] = 1
print(pallet)
if pallet[assigned_index+1] == 2:
handle_block_mismatch(block_size, assigned_index+2)
return
handle_block_mismatch(block_size, assigned_index+1)
else:
pick(top_pallet[end])
place(top_pallet[buffer]) # 미리 지정된 임시 위치
pick(top_pallet[start])
place(top_pallet[end])
print(pallet)
pallet[buffer] = 0
print(pallet)
pallet[start] = 2
print(pallet)
pallet[end] = 1
print(pallet)
if rclpy.ok():
movej(JReady, vel=VELOCITY, acc=ACC)
release()
movel(trans(x11, [0,0,100,0,0,0], DR_BASE, DR_BASE ), vel=VELOCITY, acc=ACC)
movel(x11, vel=VELOCITY, acc=ACC)
grip()
movel(trans(x11, [0,0,100,0,0,0], DR_BASE, DR_BASE ), vel=VELOCITY, acc=ACC)
movel(trans(k11, [0,0,100,0,0,0], DR_BASE, DR_BASE ), vel=VELOCITY, acc=ACC)
movel(k11, vel=VELOCITY, acc=ACC)
release()
movel(trans(k11, [0,0,100,0,0,0], DR_BASE, DR_BASE ), vel=VELOCITY, acc=ACC)
grip()
while 0 in pallet:
print(pallet)
block_size, assigned_index = check(top_pallet[pallet.index(0)])
if pallet.index(0) < 3:
if block_size == "s": pallet[pallet.index(0)] = 1
else:
handle_block_mismatch(block_size, assigned_index)
elif pallet.index(0) >=3 and pallet.index(0) <6:
if block_size == "m": pallet[pallet.index(0)] = 1
else:
handle_block_mismatch(block_size, assigned_index)
else:
if block_size == "l": pallet[pallet.index(0)] = 1
else:
handle_block_mismatch(block_size, assigned_index)
release()
movel(trans(k11, [0,0,100,0,0,0], DR_BASE, DR_BASE ), vel=VELOCITY, acc=ACC)
movel(k11, vel=VELOCITY, acc=ACC)
grip()
movel(trans(k11, [0,0,100,0,0,0], DR_BASE, DR_BASE ), vel=VELOCITY, acc=ACC)
target = top_pallet[pallet.index(2)]
movel(trans(target, [0,0,100,0,0,0], DR_BASE, DR_BASE ), vel=VELOCITY, acc=ACC)
movel(target, vel=VELOCITY, acc=ACC)
release()
movel(trans(target, [0,0,100,0,0,0], DR_BASE, DR_BASE ), vel=VELOCITY, acc=ACC)
rclpy.shutdown()
if __name__ == "__main__":
main()'두산 로보틱스 부트캠프 ROKEY > 실무 프로젝트' 카테고리의 다른 글
| [협동] ROS-2 프로그램을 활용한 협동로봇 동작 운영 실습 4일차 (0) | 2025.01.01 |
|---|---|
| [협동] ROS-2 프로그램을 활용한 협동로봇 동작 운영 실습 3일차 (0) | 2025.01.01 |
| [협동] ROS-2 프로그램을 활용한 협동로봇 동작 운영 실습 1일차 (0) | 2025.01.01 |
| [협동] RVIZ2와 Gazebo를 활용한 다중 로봇 자율 협력 시스템 시뮬레이션 5일차 (0) | 2024.12.24 |
| [협동] RVIZ2와 Gazebo를 활용한 다중 로봇 자율 협력 시스템 시뮬레이션 4일차 (0) | 2024.12.23 |
'두산 로보틱스 부트캠프 ROKEY/실무 프로젝트' Related Articles
more
