[Feature] safety zone for drawer collision

[kimseoungyun]

✨ 기능 설명

서랍 내부 영역을 측정하고, 로봇팔과 그리퍼가 서랍 벽/칸막이/바닥과 충돌하지 않도록 안전한 작업 영역과 접근 경로를 설정한다.

공구를 집기 전에 서랍의 크기, 위치, 경계, 내부 여유 공간을 인식하거나 설정값으로 관리하고, 해당 영역 안에서만 grasp pose와 gripper open width를 선택하도록 개선한다.

🎯 필요한 이유

서랍 안의 공구를 집을 때 로봇팔이나 그리퍼가 서랍 벽, 칸막이, 바닥과 충돌할 위험이 있다.
특히 공구 위치가 서랍 가장자리와 가깝거나, 그리퍼를 크게 벌려야 하는 경우 충돌 가능성이 커진다.

서랍 영역을 명확히 측정하고 안전 margin을 적용하면, 공구 집기 성공률을 높이고 로봇/서랍/공구 손상을 방지할 수 있다.

🔄 제안하는 동작 흐름

1. 사용자가 음성으로 요청:

   • 예: “망치 가져와”, “드라이버 집어줘”

2. 시스템이 공구/상태를 판단:

   • perception 모듈이 대상 공구 위치를 감지한다.
   • 서랍의 내부 영역, 벽, 칸막이, 바닥 위치를 측정하거나 설정값에서 불러온다.
   • 공구의 grasp pose 후보가 서랍 안전 영역 안에 있는지 확인한다.
   • 그리퍼를 벌렸을 때 주변 서랍 구조물과 충돌 가능성이 있는지 판단한다.

3. 로봇팔/서랍/그리퍼/전달 로직 수행:

   • 안전 margin을 적용한 서랍 작업 가능 영역을 계산한다.
   • 충돌 위험이 낮은 접근 방향과 grasp pose를 선택한다.
   • 필요한 경우 gripper open width를 서랍 내부 여유 공간에 맞게 제한한다.
   • MoveIt planning scene 또는 별도 collision check에 서랍 영역을 반영한다.
   • 안전한 경우에만 로봇팔이 접근하고 공구를 집는다.
   • 이후 기존 pick/return/handover 플로우를 수행한다.

4. 완료 또는 실패 처리:

   • 안전 영역 내에서 공구를 정상적으로 집으면 완료 상태를 남긴다.
   • 공구가 서랍 벽과 너무 가깝거나, gripper clearance가 부족하거나, 경로 계획 중 충돌 위험이 있으면 grasp를 시도하지 않고 실패 처리한다.
   • 실패 이유를 로그와 상태 메시지로 남긴다.

✅ 완료 조건

• 서랍 내부 영역 또는 경계 정보를 측정하거나 설정값으로 관리한다.
• 서랍 벽/칸막이/바닥에 대한 안전 margin이 적용된다.
• 공구 grasp pose가 서랍 안전 영역 안에 있는지 검사한다.
• gripper open width가 서랍 내부 clearance를 초과하지 않도록 제한된다.
• 충돌 가능성이 있는 접근 경로 또는 grasp pose는 제외된다.
• 안전한 grasp가 불가능한 경우 실패 상태로 처리된다.
• 기능이 정상 동작한다.
• 실패 상황이 처리된다.
• 로그 또는 상태 메시지가 남는다.
• 필요한 문서가 업데이트된다.

📦 관련 레포지토리/모듈

• macgyvbot
• macgyvbot-perception
• macgyvbot-datasets
• macgyvbot-docs
• macgyvbot-vla
• macgyvbot-simulation

관련 모듈:

• macgyvbot_perception
• macgyvbot_manipulation
• macgyvbot_task
• macgyvbot_config
• macgyvbot_bringup

📈 예상 난이도

• 낮음
• 중간
• 높음
• 조사 필요

📎 추가 참고 자료

• RealSense depth 기반 서랍 내부 영역 측정 가능 여부 확인 필요
• 서랍 geometry를 MoveIt planning scene collision object로 등록하는 방식 검토
• 서랍 내부 안전 margin, gripper collision margin, 접근/후퇴 방향 설정 필요
• 공구가 서랍 벽에 너무 가까운 경우 대체 grasp pose 선택 또는 실패 처리 필요
• 관련 파일 후보:
  • src/macgyvbot_perception/macgyvbot_perception/hand_tool_grasp/tool_detector.py
  • src/macgyvbot_manipulation
  • src/macgyvbot_task/macgyvbot_task/task_coordinator_node.py
  • src/macgyvbot_bringup/launch/macgyvbot.launch.py