융합_로보테크 AI 자율주행 로봇 개발자 과정-26/06/18

PACK 로봇팔 자율 잡기 — 2026-06-18 작업

목적: 이 md 하나로 다음 세션이 진행 상태 완전 복원하고 작업 이어서 가능.
최종 갱신: 2026-06-18 (단일 박스 자율 잡기 부분 작동, 거리별 캘리브 정밀화 미완)

---

0. 작업 환경 (재시작 시 첫 단계)

우분투 PC (작업)

• 경로: /home/cho/Downloads/PACK/ — 모든 코드, yaml, md
• /home/cho/RobotArmCase/ — 별도 사본 (옛 backup)

라파이 (실행)

• 접속: ssh group4@192.168.0.187 (비번 없음, 키 등록)
• 홈: /home/group4/ (= ~)
• 파일: 우분투에서 scp로 푸시 → 라파이 ~/에 위치
  • box_pick.py, box_detect_3d.py, arm_fk.py, arm_ik.py, pose_probe.py
  • camera_intrinsic.yaml, hand_eye.yaml

서비스 시작 (라파이)

ssh group4@192.168.0.187 'bash ~/start_all.sh'

• camera_node (camera:=1 1640x1232 RGB888)
• mqtt_gateway (Arduino by-id port)
• bringup (필요 X 이면 무시)

자세 확인

ssh group4@192.168.0.187 'tail -5 ~/logs/arm.log'        # arm 응답
ssh group4@192.168.0.187 'ps -ef | grep mqtt_gateway'    # gateway 떠있나

중복 게이트웨이 주의: 2개 실행 시 시리얼 충돌 — 하나만 유지.

---

1. 프로젝트 목표 (사용자 명시)

단일 박스 자율 잡기 → 후방 놓기 → 3개 박스 자동화

• 박스 = 검은 도넛형 prism (옆면 주황 띠 마커, 윗면 작음)
• 받침대 = 흰 큰 박스 (TurtleBot 박스), 윗면 빨간 테이프
• 박스 3개 받침대 위에 일렬 (좌중우)
• 잡고 후방(베이스 회전축 기준 base 150° 부근) 후방 차량 적재 위치에 놓기

자율성 요구

• 거리 변동: 어깨박스 **1520cm** 범위, ±2.5cm 오차 자동 보정
• 각도 변동: 박스 회전 ±10도 자동 보정
• 11자 정렬: 주행으로 보장
• 카메라 기반 동적 위치 추정 (하드코딩 또는 사람 조종 X)

---

2. 캘리브 데이터 (확정값)

측정.docx (기존)

• 링크: L1=105, L2=80, L3=125 mm
• TCP z 오프셋 = -25 mm (provisional)
• base radial offset = 30 mm (provisional)
• base z offset = 14 mm (provisional)
• 채널 변환: base_deg=CH15-37, shoulder=CH13-10, elbow_rel=-CH11, wrist_rel=CH9-170
• IK: elbow_up 기본

카메라 intrinsic (camera_intrinsic.yaml)

• 이미지 1640x1232
• fx=1334.09, fy=1330.48
• 광축 cx=835.27, cy=653.15 (화면 정중앙 820, 616에서 +15, +37)
• 왜곡: k1=0.20, k2=-0.33
• RMS=0.25px (검출 12/15 자세)
• 시야: 수평 63°, 수직 50°
• 체크보드: 9×7 칸 (내부 코너 8×6), 칸 24mm
• 캡쳐 스크립트: camera_calib_capture.py (라파이에서 sweep)
• 분석: camera_intrinsic_calib.py (PC에서 OpenCV)

Hand-eye (hand_eye.yaml) — 자로 직접 측정

• 측정 자세: CH13=120 CH11=60 CH9=170 CH7=130 (그리퍼 일자)
• 측정값:
  • 카메라가 TCP보다 5.5cm 뒤
  • 카메라가 정중앙선에서 왼쪽 0.5cm
  • 카메라가 TCP보다 2.5cm 낮음
• t_cam_to_tcp_mm: [-55.0, 5.0, -25.0] (TCP 좌표계 cam 원점 위치, OpenCV convention)
• R_cam2tcp: 단위 좌표축 매핑만 (카메라 회전 무시 가정)

  [[ 0,  0,  1], [-1,  0,  0], [ 0, -1,  0]]

• 자동 hand-eye 캘리브는 부정확 (gripper2base 회전 모델 부정확). 자로 측정값 사용 중. ⚠️ 정밀화 필요 (다음 작업).

셋업 측정값 (사용자 자로 측정)

• 받침대 윗면: 어깨축 기준 -117 mm → 베이스축 기준 -103 mm (FK z)
• 박스 모양: 삼각 단면 prism, 높이 5.2cm, 바닥 1.6cm × 6.2cm
• 박스 옆면 띠: 빗변(기울어진 면)에 부착, 박스 중간 높이 z=-77mm
• 박스 윗점(꼭지점): z=-51mm
• 잡기 위치 z = -78mm (받침대 + 2.5cm = 도넛 구멍 안)
• 그리퍼 도넛 구멍 입력 깊이: 받침대 끝에서 앞 3.1cm 위치

---

3. 코드 구조

arm_fk.py — 정기구학 ✅

• FK: 채널값 → TCP 베이스 좌표 (x, y, z, tcp_pitch)
• 회전 매트릭스 (사용자 좌표계: x=앞, y=좌, z=위)
• 측정.docx 8/9/11절 케이스 검증 PASS

arm_ik.py — 역기구학 ✅

• IK: (x, y, z, tcp_pitch) → Pose(CH15, CH13, CH11, CH9)
• elbow_up 기본
• 안전범위 + 충돌 가드 + workspace 체크
• 측정.docx 10/11/13절 케이스 검증 PASS (반올림 오차 0.7mm 이내)

box_detect_3d.py — 카메라 픽셀 → 3D 좌표 ✅

• 색 검출 (주황 박스 옆 / 빨강 받침대)
• 픽셀 → 카메라 광선 (undistortPoints 사용)
• 광선 + 평면 z=-77 → 박스 좌표 (베이스 기준)
• 부호 수정: arm_fk와 일관성 위해 pitch = -tcp_abs_deg 사용 (Ry 표준 부호 반대)
• 좌표 변환: t_cam_in_base = t_tcp + R_tcp @ t_cam2tcp (OpenCV convention)

box_pick.py — 2단계 자율 잡기 (메인)

• Stage 1: 어깨 들기 자세에서 1차 검출 (박스 대략)
• Stage 2: 박스 위 80mm 자세 (IK)에서 2차 검출 (정밀)
• Stage 2b: cx 정렬 (visual servoing, K_BASE=0.7/29)
• Stage 3: 잡기 IK (--grab-z, --tcp-pitch)
• Stage 4: 잡기 자세 안전 시퀀스 (베이스→팔꿈치→손목→어깨 순)
• Stage 4b: 잡기 직전 cx 재정렬 (어깨 내리기 전)
  • ⚠️ 현재 박스 시야 밖이라 작동 안 함 — 어깨를 들기 유지하고 다른 채널만 잡기 자세 변경하는 방식으로 수정 필요
• Stage 5: 그리퍼 닫기 + 들기
• Stage 6: 후방 회전 + 놓기 + 홈

pose_probe.py — 자세 sweep + 색 검출 (디버그) ✅

• 인자: --sh, --wrist, --base, --elbow, --roll, --seconds, --color, --debug
• 결과: ~/pose_probe.jpg + 콘솔 통계
• detect_color_center cx_target 인자: cx 정중앙 가까운 contour 선택

ik_pick.py — 좌표 입력 IK 도구 (옛 도구) ✅

캘리브 도구

• camera_calib_capture.py (라파이): 자세 sweep + 영상 캡쳐 + poses.csv 저장
• camera_intrinsic_calib.py (PC): OpenCV chessboard 캘리브
• camera_handeye_calib.py (PC): hand-eye 캘리브 ⚠️ gripper2base 모델 부정확 — 결과 신뢰 X

---

4. 잡기 성공 사례 데이터 (오늘 측정)

거리 (사용자측정)	잡기 자세 (CH15/CH13/CH11/CH9)	FK 그리퍼 (x,y,z)	tcp_pitch	잡힘
19cm	47 / 37 / 30 / 130	(273, 48, -46)	-43°	✅ 사용자 미세조정
20cm	48 / 35 / 39 / 154	(244, 48, -46)	-30°	✅ 자동 (Stage 2b 정렬 후)
17cm	46 / 42 / 30 / 120	(261, 41, -37)	-48°	✅ 사용자 미세조정. 자동 X

x_offset 변동 (검출 좌표 → 실제 잡기 좌표 보정량, x 축)

• 19cm 자세: 검출 354 → 실제 273 → x_offset = -81
• 20cm 자세: 검출 244 → 실제 244 → x_offset = 0
• 17cm 자세: 검출 230 → 실제 261 → x_offset = +31
• 부호 변경됨 → hand-eye 회전 매트릭스 오차 + 박스 평면 z 가정 오차 누적이 원인

거리-자세 추세

• 거리 멀어질수록: 어깨 더 들고(CH13↑), tcp_pitch 작아짐(절댓값↓)
• 거리 가까이: 어깨 내리고 tcp_pitch 큼

---

5. 작동 흐름 (사용자 만족 케이스)

20cm 자동 잡기·놓기 (성공)

ssh group4@192.168.0.187 'bash -lc "source /opt/ros/humble/setup.bash && python3 ~/box_pick.py --grip --tcp-pitch -30"'
# 옵션: --place (후방 놓기 추가)

Visual servoing 작동 ✅

• Stage 2b: cx 4번 반복 후 ±25px 안 정렬
• 다중 박스 중 cx 정중앙 가까운 contour 선택 (좌측 박스 잘못 잡는 문제 해결)

---

6. 미해결 문제 (다음 작업 시급)

문제 1: 자세별 x_offset이 매번 다름

• 검출 좌표 → 잡기 좌표 매핑이 자세별로 변동 (-81 → 0 → +31)
• 원인: hand-eye 회전 매트릭스 오차 + 박스 옆면 띠 평면 z=-77 가정 오차

문제 2: 17cm 거리 자동 잡기 실패

• IK 결과 (CH15=44/CH13=62/CH11=77/CH9=147) vs 사용자 자세 (46/42/30/120) 차이 큼
• Stage 4b 검출 실패 — 잡기 자세에서 박스 시야 밖

문제 3: 1차 검출 자세별 박스 시야 변동

• CH13=120 + CH9=80: 박스 17cm 부근 못 봄
• CH13=120 + CH9=70: 박스 17cm 잡힘
• CH9 자세 따라 검출 자세 sweep 가능

문제 4: hand-eye 자동 캘리브 부정확

• camera_handeye_calib.py 결과: t=(117, 10, -12) RPY=(52, 70, 58)
• 자로 측정값 (-55, +5, -25) 회전 단위와 큰 차이
• 원인: gripper2base 회전 매트릭스가 yaw×pitch×roll 단순 분해 — 손목 회전축 정확치 않음

---

7. 다음 작업 (우선순위)

우선 — Phase B: Hand-eye 정밀화

1. gripper2base 회전 모델 정확화
   • 손목 회전(CH7) 처리 정확히 — handoff: 40=세로, 130=가로, 90 중립
   • 회전축 방향이 손목 자세에 따라 변동 — 정확한 운동학 변환
2. 다양한 자세 데이터 재수집 (CH13/CH11/CH7 다양화)
3. 자동 캘리브 다시 + 검증 (예: 사용자 자세 FK 결과와 매치 확인)
4. 결과로 hand_eye.yaml 정밀 업데이트

동시 진행 — Phase A: Stage 4b 개선

• 잡기 자세에서 박스 안 보이는 문제
• 해결안:
  1. 어깨는 들기(CH13=120) 유지 + 다른 채널만 잡기 자세 변경
  2. 그 자세에서 cx/cy 검출 + 정렬
  3. 어깨 점진 내림 (수직 하강)
• 어쩌면 별도 entry pose IK (잡기 위 30mm) — unreachable 가능, fallback 필요

Phase C: 모델 보정 데이터 회귀

• 거리별 x_offset, tcp_pitch 함수 학습
• 데이터: 17/19/20cm 외 더 측정 (15, 16, 18, 21, 22cm)
• 회귀: x_offset(distance), optimal_tcp_pitch(distance)

Phase D: 3개 박스 처리 (Phase A~C 완료 후)

• 사용자 셋업: 박스 3개 일렬
• box_pick에 sweep 추가: base sweep으로 3개 박스 차례로
• 한 박스 잡기 후 후방 놓기 → 다음 박스 위치 → 반복 3회

Phase E: 각도 변동 ±10° 대응 (3개 박스 후)

• 박스 회전 검출 (bounding rect angle)
• 그리퍼 손목 회전(CH7)으로 매치

---

8. 코드 호출 예시 (인수인계용)

단일 박스 자동 잡기 (20cm)

ssh group4@192.168.0.187 'bash -lc "source /opt/ros/humble/setup.bash && python3 ~/box_pick.py --grip --tcp-pitch -30"'

17cm 수동 자세 (검증된 사용자 미세조정)

ssh group4@192.168.0.187 'mosquitto_pub -h localhost -t pack/arm/cmd -m "R"; sleep 0.3
mosquitto_pub -h localhost -t pack/arm/cmd -m "13 120"; sleep 5
mosquitto_pub -h localhost -t pack/arm/cmd -m "11 30"; sleep 2
mosquitto_pub -h localhost -t pack/arm/cmd -m "9 120"; sleep 3
mosquitto_pub -h localhost -t pack/arm/cmd -m "7 130"; sleep 1
mosquitto_pub -h localhost -t pack/arm/cmd -m "15 46"; sleep 4
mosquitto_pub -h localhost -t pack/arm/cmd -m "13 42"; sleep 4
mosquitto_pub -h localhost -t pack/arm/cmd -m "5 20"; sleep 2'

19cm 수동 자세

• CH15=47, CH13=37, CH11=30, CH9=130, CH7=130, CH5=20

후방 놓기 (수동)

ssh group4@192.168.0.187 'mosquitto_pub -h localhost -t pack/arm/cmd -m "15 150"; sleep 4
mosquitto_pub -h localhost -t pack/arm/cmd -m "13 60"; sleep 4
mosquitto_pub -h localhost -t pack/arm/cmd -m "9 90"; sleep 2
mosquitto_pub -h localhost -t pack/arm/cmd -m "5 0"; sleep 2
mosquitto_pub -h localhost -t pack/arm/cmd -m "9 170"; sleep 2
mosquitto_pub -h localhost -t pack/arm/cmd -m "13 120"; sleep 4
mosquitto_pub -h localhost -t pack/arm/cmd -m "H"; sleep 5'

캘리브 캡쳐 (라파이 자세 sweep)

ssh group4@192.168.0.187 'rm -f ~/calib_imgs/*.jpg'
ssh group4@192.168.0.187 'bash -lc "source /opt/ros/humble/setup.bash && python3 ~/camera_calib_capture.py --base 35 40 45 --sh 120 --wrist 90 100 110 120 130 --elbow 60 --roll 130"'
# 다운로드
scp -q group4@192.168.0.187:~/calib_imgs/*.jpg /home/cho/Downloads/PACK/calib_imgs/

---

9. 사용자 사용 가이드 / 약속

그리퍼 상태

• 글루건 고정됨 (그리퍼 중간 관절 안 움직이게)
• 닫힘값 20 (새 집게)
• 열림값 0

자세 명령 발행 시 주의

• CH13 어깨 큰 회전 후 충분 dwell (3~5초)
• CH15 베이스 회전 후 5초 권장 (시리얼 큐 누락 방지)
• 멀티 채널 한번에 발행하면 일부 명령 누락 — 순차 발행 + sleep

펌웨어 EMS 주의

• 명령 "0" = EMS 활성. 이후 명령 다 거부.
• 해제: "R" 발행
• box_pick.py 시작 시 자동 "R" 발행 (해제됨)

안전 시퀀스 (사용자 강조)

• 베이스 회전 시 어깨 들기 자세 유지 (그리퍼가 박스 옆 호로 휘둘러 치는 일 방지)
• 어깨 내림은 마지막에 (수직 하강)
• 잡기 시퀀스 순서: 베이스 → 팔꿈치 → 손목 → 어깨

---

10. 좌표계 정의

베이스 좌표계 (FK 기준)

• 원점: CH15 회전축 중심 (디스크 윗면)
• x = 앞 (그리퍼가 base_deg=0일 때 향하는 방향)
• y = 좌
• z = 위

TCP 좌표계

• 베이스 좌표계와 같은 축
• 원점: 그리퍼 끝 (두 손가락 만나는 점)

카메라 좌표계 (OpenCV 표준)

• 원점: 카메라 렌즈 중심
• x = 이미지 오른쪽
• y = 이미지 아래
• z = 카메라 광축 (앞)

hand-eye 변환

• P_tcp = R_cam2tcp @ P_cam + t_cam2tcp (OpenCV convention)
• t_cam2tcp = TCP 좌표계에서 본 카메라 원점 위치

---

11. 베이스(CH15) 좌표 정정 이력

• 측정.docx: base_deg = CH15 - 37 → CH15=37이 정면(base_deg=0)
• handoff 옛 표 "0=왼, 90=뒤, 180=오른" → 부정확
• 사용자 셋업: 박스가 base 25~70 영역 (정중앙 ≈ 47), 후방 = base 150
• 일관성 위해 box_pick은 scan_ch15 인자로 받음 (셋업 변경 시 조정)

이전 인수인계: /home/cho/Downloads/HANDOFF_for_claude_code.md (옛 visual_pick 흐름)