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

2026-06-24 작업 정리

박스 색 배치 정정 (HSV 검출용 새 박스)

• 윗면 = 주황 (13×100 좁은 직사각형) → minAreaRect 중심/거리/회전
• 정면(그리퍼 도달 방향 면) = 파랑 (13×60) — 좌우 옆면 아님. orange_blue_detect.py docstring/주석 "옆면(좌우)" → "정면" 으로 수정
• detect_blue_centers 두 면 평균 → anchor cx 가장 가까운 정면 1개로 수정
• HSV: 주황 (5,120,120)-(22,255,255), 파랑 (95,100,80)-(130,255,255)
• 거리 기준 = 터틀봇 바퀴 ~ 받침대 끝 (자로 잰 17cm). 검출 베이스 x ≠ 사용자 거리.

단독 잡기 (auto_pick_new.py) — C/L/R 17cm 전부 성공

• 흐름: 검출 자세 → box_detect_3d cx 정렬 → orange_blue_detect 중심 → box_front=중심+50 → ik_safe(grip_z=-46, pitch=-42) → 도달+그리퍼
• C (233,-5), L (228,+37), R (228,-52) 모두 잡힘

3박스 연속 시퀀스 (multi_pick_v4.py 신규)

auto_pick_new 단일 흐름을 자리별 루프로 감쌈. 자리별 시작 CH15 = {C:47, L:58, R:33}.
후방 놓기(트레이): CH15=150, CH9=90, CH13=60, 그리퍼 열기.

R 검출 버그 진단 (핵심)

증상: 시퀀스에서 R만 검출 (228,-52) 대신 (~210, -15)로 나와 grab 실패. 3번 재현.

가설 검증:

1. 빨간 배경 오검출? → 기각. 주황 마스크 완전 새까맘 (S/V≥120이라 어두운 빨강 안 걸림)
2. C·L 잡는 동작이 R 물리적으로 밀침? → 기각. 사용자 확인 "R 한 번도 안 밀림". 클러터 상태(3박스) 단독 R 검출 = (226,-57) 정상
3. 카메라 자세 미정착 → 확정.
   • 놓기 자세(CH15=150,CH9=130,CH13=60)에서 R 검출 자세로 올 때 카메라가 덜 펴져 박스가 프레임 바닥에 잘림 (주황 짧은변 141→12~69px) → minAreaRect 중심 왜곡 → y 틀림
   • R 리셋 0.5s vs 3.5s 비교 → 둘 다 틀림 (리셋 시간 무관)
   • 2-pass (검출 자세 명령 CH11/CH9/CH13 재발행) → (226.7, -53.6) 정상 복구
   • 원인 = 오픈루프 MG90S가 먼 자세에서 1회 명령으론 목표각 미달

적용한 3가지 해결책 (multi_pick_v4.py)

1. 자리 검증 가드: EXPECTED_Y {C:0, L:+32, R:-55}, 허용 편차 25mm. 초과 시 "밀림/오검출 감지" → 그 박스 잡기 중단 (엉뚱한 grab 차단)
2. 박스마다 R 리셋: move_detect_pose 시작에 R — 단독 성공 경로와 동일하게
3. 2-pass 자세 명령: 검출 자세 1차 명령 후 CH11=45/CH9=90/CH13=80 재발행 → 카메라 정착

최종 결과 (2-pass 적용)

박스	검출 (x,y)	y편차	IK (15/13/11/9)	결과
C	(234.3, -2.8)	-2.8	36/31/17/124	✓
L	(229.6, 35.8)	+3.8	44/34/25/129	✓
R	(227.5, -53.4)	+1.6	26/33/23/128	✓

3개 다 잡고 후방 트레이 놓기 완료.

박스 바닥 보강 → 클린 성공

• 1차 성공 땐 C 잡을 때 얇은 L(폭 13mm) 쓰러져서 사용자가 다시 세움 (애매)
• 박스 바닥 쪽만 더 넓게 보강 → 재실험
• 보강 후 풀 CLR 시퀀스: 검출 C(232,-8) L(232,+29) R(227,-58) 전부 가드 통과, 3개 다 건드림/쓰러짐 없이 클린하게 잡고 놓기 완료
• 3박스 17cm 연속 시퀀스 완성

15cm 3박스 시퀀스 — 클린 성공

17cm용 가드(EXPECTED_Y C:0/L:32/R:-55, TOL 25) 그대로 15cm도 y편차 전부 작아 통과 (조정 불필요).

박스	검출 (x,y)	y편차	IK (15/13/11/9)
C	(227.1, 6.2)	+6.2	38/39/36/135
L	(225.6, 37.6)	+5.6	45/38/34/134
R	(225.5, -53.7)	+1.3	26/36/28/130

• 드라이런 때 L x=192 극단 IK(CH11=78) → 풀런에서 재발 X (일회성 검출 글리치)
• R cx 잔차 -36으로 ✓ 미달이나 y 정상, 잡힘
• 15cm 3개 다 클린하게 잡고 놓기 완료

거리별 검출 x 거의 변화 없음 (15cm·17cm 모두 C/L/R x≈225234) — 사용자 거리(터틀봇받침대)와 박스 베이스 x 약결합.

19cm 3박스 시퀀스 — reach 한계, marginal

• pitch=-42, grip_z=-46로는 C·R IK unreachable (D=186~188 > L1+L2=185). 예전 19cm 검증은 다른 자세(더 눕힘)였음.
• reach 연장: pitch/grip_z 스윕 → pitch=-35가 -42에서 가장 적게 바꾸며 C·L·R 다 닿음 (-25 이하는 safe range 벗어남)
• multi_pick_v4.py에 --pitch, --grip-z 인자 추가 (기본 -42/-46). 19cm는 --pitch -35로 실행
• 결과: 셋 다 잡히긴 했으나 그리퍼가 중앙 구멍이 아니라 near-side 기둥을 물어 운반 불안정. 원인 = reach 한계라 TCP가 box_front(center+50)까지 못 펴짐 → near-side에서 닫힘. flatter pitch는 악화, 더 깊이는 reach 초과 → 소프트 해결 불가.

결론: 19cm = reach 한계 → 아래에서 해결됨 (reach 아니라 자세 문제였음).

거리별 검출 x: 15cm≈225227, 17cm≈228234, 19cm≈231~240 (거리 멀수록 x 약간 증가).

19cm 해결 — 자세(redundancy) 문제였음

reach 한계가 아니라 IK 자세 선택 문제였다 (사용자 직관: "팔 길이 문제 아니라 자세 문제, CH9 거의 170이어야"). 진단·해결 과정:

1. 고정 pitch의 함정: pitch=-42 딱 고정 → R/C가 0.5°/1mm 차이로 unreachable. 거기서 -35로 확 눕히니 접근각 망가져 near-pillar 물음. → pitch 고정 말고 redundancy(13/11/9) 활용해야.
2. near-pillar 원인 = 손목 굽음: steepest pitch는 CH11≈8(직선)이지만 CH9≈120(손목 위로 말림) → 그리퍼가 앞 기둥 침. CH9를 170으로 펴면 그리퍼가 forearm 따라 앞으로 뻗어 중앙으로 들어감.
3. maxch9 IK 모드 추가: redundancy를 "steepest pitch"가 아니라 CH9 최대(손목 최대 폄)로 해소. (IK_MODE='maxch9', ik_auto가 pitch 스캔해 CH9 최대 자세 선택)
4. offset(잡는 깊이) 키우기: offset 50은 maxch9라도 CH11=62로 굽어 sag→짧음. offset 80 → CH9170 유지 + CH1135로 펴짐 → sag 적어 reliably 도달 + 그리퍼 깊이 들어가 중앙 그립.
5. grip_z -46→-50: 상단 모서리 말고 몸통 살짝 아래 잡아 안정.
6. 그리퍼 회전 무관: L이 가끔 앞면 쳐 쓰러뜨렸으나 marginal(variance), 재시도로 성공.
7. R 멀티박스 오검출 수정: orange_blue_detect.detect_orange_rect가 max-area라 3박스 중 그리퍼에 가린 엉뚱한 박스 집음(x=296 등). anchor_cx 최근접 선택으로 수정 (box_detect_3d는 이미 그러함). 이걸로 R x오검출 해결.

최종 19cm 설정: multi_pick_v4.py --front-offset 80 --grip-z -50 (IK_MODE=maxch9 기본).
3박스 결과: C(234.9,-13.8) L(234.6,28.9) R(232.1,-57.5), 자세 CH1325-26/CH1133-36/CH9=170/pitch~-20. C·L·R 전부 잡고 놓기 성공.

놓기 동작 개선

놓기 동작은 여러 번 반복 개선 (수평신전 CH11=0 → z 스파이크/흔들림 문제) 끝에 잡은 자세(CH11/CH9) 유지 + 어깨만 들었다 내리기로 확정. 아래 "최종 놓기" 참조.

거리 자율 — offset 80 통일 (거리 추정 불필요)

검출 x는 1519cm에서 ~8mm밖에 안 변해(노이즈 ±수mm) → x기반 거리추정은 불안정. 대신 발견: maxch9 regime에선 offset 80일 때 pitch가 거리 무관 ~-20°로 일정(15cm -21-23, 17cm -20, 19cm -19). pitch 일정 = 그리퍼 finger-trail 일정 → offset 80 하나로 15~19cm 전부 중앙 그립. reach도 15cm offset 80까지 도달 OK.
→ 기본값 offset 80 / grip-z -50 / IK_MODE=maxch9. 거리 랜덤이어도 그냥 multi_pick_v4.py 실행. (검증: 거리 섞어 배치 → 전부 성공)

최종 놓기 동작 (흔들림·충돌 회피)

• 잡은 자세(CH1138, CH9170) 유지, 어깨(CH13)만 움직여 z 출렁임 제거.
• 순서: 잡기 → CH13 74 들기(z175, 풀들기 120의 절반 — 뒤 추종차량 옆면 위로 옮김) → CH15 150 스윙 → CH13 20 하강(z-77) → 그리퍼 열기 → CH13 120 치우기.
• 놓기 순서 LCR(시작 CH15 내림차순 58→47→33): 놓기 스윙은 항상 CH15 150으로 가는데, 내림차순이면 매 스윙이 남은 박스보다 바깥으로 가 절대 안 지나감 (CLR이면 C 놓기 스윙이 L을 침 → 발견·수정).

동작 범위 / 한계

• 15~19cm 임의 거리 3박스 시퀀스 성공 (offset 80 통일, 거리 자율).
• 받침대 회전(틀어짐) 대응은 범위 외로 포기 (CH7 회전 자동매핑 안 함). 박스는 받침대 모서리에 나란히 정자세 전제.

전체 동작 시나리오 (multi_pick_v4.py)

전제: 라파이(group4@192.168.0.187)에서 카메라(camera_ros, /camera/image_raw, ROS_DOMAIN_ID=0)·mqtt_gateway·mosquitto 기동. 박스 3개를 받침대 모서리 C/L/R 자리에 정자세로(거리 15~19cm 임의) 세움. 실행: unset ROS_DOMAIN_ID; python3 multi_pick_v4.py.

1. 초기화: 카메라 intrinsic 로드, cx_target=광축(835). "R" 리셋 + 그리퍼 열기 + 어깨 안전(120).
2. 박스별 루프 (순서 LCR) — 각 박스마다:
   • [검출 자세] move_detect_pose(start_ch15): R 리셋(매 박스 — 자세 정착 일관) → 그리퍼 열기 → 어깨120 → CH15=자리시작값(L58/C47/R33) → CH11=45 → CH7=130 → CH9=90 → CH13=80 → 2-pass(CH11/CH9/CH13 재발행)(오픈루프 서보 undershoot 보정).
   • [cx 정렬] box_detect_3d(주황, anchor_cx 최근접)로 박스를 광축에 맞춰 CH15 미세조정(K=0.025, ≤25px, 최대 8회).
   • [위치 측정] orange_blue_detect(주황 윗면 minAreaRect, anchor_cx 최근접 — 멀티박스에서 타겟 박스 일관 선택)로 박스 중심 베이스좌표 (x,y,z=-41).
   • [자리 검증 가드] EXPECTED_Y{C:0,L:32,R:-55}±25mm. 벗어나면 "밀림/오검출 감지" → 그 박스 잡기 중단(엉뚱한 grab 차단).
   • [IK] box_front = (중심x + 80, y, -50). ik_auto(maxch9): pitch 스캔해 CH9 최대(손목 최대 폄) 자세 선택 → 그리퍼가 박스 중앙까지 뻗음.
   • [잡기 도달] 어깨120 → CH15(cx정렬값) → CH11 → CH9 → CH13(IK) → 2-pass → 그리퍼 닫기.
   • [놓기] CH13 74 들기 → CH15 150 스윙 → CH13 20 하강 → 그리퍼 열기 → CH13 120.
3. 완료.

핵심 파라미터(multi_pick_v4.py 상단): DETECT_POSE(13=80,11=45,9=90,7=130), BOX_START_CH15{C:47,L:58,R:33}, EXPECTED_Y{C:0,L:32,R:-55}, Y_TOL=25, BOX_DEPTH_HALF=80, GRIP_Z=-50, IK_MODE='maxch9', 기본 --order LCR. 거리별 조정 인자: --front-offset, --grip-z, --pitch(steep 모드용).

파일 상태 (워크스테이션 ~/Downloads/PACK = 라파이 /home/group4 동기화)

• multi_pick_v4.py — 신규, 3박스 시퀀스 (가드 + R리셋 + 2-pass)
• auto_pick_new.py — 단일 잡기 (검증됨)
• orange_blue_detect.py — 정면 파랑 수정본
• 진단 이미지: L_annot, R_diag2, R_seqfail, R_clean, R_testC/D 등

환경 메모

• 카메라 camera_ros는 도메인 0에 발행 (start_all.sh가 ROS_DOMAIN_ID export 안 함). 검출 스크립트는 unset ROS_DOMAIN_ID로 돌려야 함 (bashrc는 199)
• 라파이 = group4@192.168.0.187 (터틀봇 .188과 다름)