그리퍼(Gripper)까지 붙였으니 이제 실제로 물체를 집어 옮기는 동작을 만들 차례다. 접근 → 하강 → 파지 → 들어올리기 → 이동 → 하강 → 해제 순서로 작성해 보았다.
실제 작업 중 발생한 문제들에 대해 정리해 보았다.
1. PATH_TOLERANCE_VIOLATED: 시뮬레이션 바닥 충돌
증상
임의의 좌표로 이동 명령 시 다음과 같이 경로 허용 오차를 위반했다는 오류 메시지를 출력하면서 로봇이 멈추었다.
[ERROR] [moveit.ros.move_group_interface]: MoveGroupInterface::move() failed or timeout reached
[ERROR] [arm_control_app]: Move to position failed동일한 시간에 출력된 MoveIt 컨트롤러의 출력은 다음과 같았다.
[moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[WARN] [follow_joint_trajectory_controller_handle]: Controller 'joint_trajectory_controller'
failed with error PATH_TOLERANCE_VIOLATED: Aborted due to path tolerance violation
[INFO] [move_group.move_action]: CONTROL_FAILED원인
"Goal request accepted!"가 출력된 것으로 보아 목표 경로를 못 찾은 것이 아니라 찾아낸 경로를 로봇이 따라가다가 문제가 발생한 것으로 판단했다.
시뮬레이션 화면을 자세히 보니 내가 지정한 목표 지점이 바닥에 바짝 붙어 있는 곳이어서 이동 중에 로봇 팔이 바닥에 부딪히면서 이동을 못하는 것으로 보였다.
좀 더 알아 보니 MoveIt의 Planning Scene은 URDF에 정의된 로봇 본체의 링크만 알 뿐, Gazebo 환경의 '바닥' 존재를 모른다. 따라서 바닥을 파고드는 궤적을 정상으로 판정해 내보냈으나, 실제 시뮬레이터(Gazebo)에서는 팔이 물리적인 바닥에 막혀 더 내려가지 못했다. 결국 궤적과 실제 관절 위치의 오차가 설정된 허용치(0.2 rad)를 초과하여 강제 중단되었다.
해결책: Planning Scene에 바닥 형상 추가
MoveIt이 바닥을 인식할 수 있도록 C++ 코드에서 Planning Scene에 BOX 형상을 직접 추가했다.
moveit::planning_interface::PlanningSceneInterface iface_ps;
moveit_msgs::msg::CollisionObject ground;
ground.id = "ground";
ground.header.frame_id = "world";
ground.primitives.resize(1);
ground.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
ground.primitives[0].dimensions = { 1.0, 1.0, 0.005 };
ground.primitive_poses.resize(1);
ground.primitive_poses[0].position.z = -0.0025; // 박스 두께의 절반
ground.operation = moveit_msgs::msg::CollisionObject::ADD;
iface_ps.applyCollisionObject(ground);- Z 좌표 오프셋 주의: 박스 모델은 중심점을 기준으로 배치된다. 두께 0.005m 박스 윗면을
z=0에 맞추려면 중심을-0.0025로 설정해야 한다.-0.005로 설정하면 바닥에 2.5mm 틈새가 생겨 로봇이 여전히 파고든다. - 동기식 반영: 비동기 토픽 방식인
addCollisionObjects()대신 서비스 호출 방식인applyCollisionObject()를 사용했다. 호출이 반환됨과 동시에 환경 반영이 보장되므로, 바로 다음 줄에서 안전하게move()플래닝을 수행할 수 있다.
2. GOAL_STATE_INVALID: 물리적 도달 불가 목표
증상
[ERROR] [RRTConnect.cpp:265]: Unable to sample any valid states for goal tree
[ERROR] [move_group]: Planner 'OMPL' failed with error code GOAL_STATE_INVALID원인
테스트용으로 입력한 목표 좌표 (0.0, 0.0, 0.1) 자체가 문제였다. 이 좌표는 베이스 관절 축 바로 위의 매우 낮은 높이로, 로봇이 자기 몸체와 부딪히지 않고는 도달할 수 없는 공간이다. 플래너가 역운동학(IK) 해를 구하지 못해 탐색 시작조차 포기했다.
확실한 좌표 도출 및 TCP 지정
머릿속 추측 대신 다음 2가지 실무적 방법으로 좌표를 구해야 한다.
1. RViz의 MotionPlanning 패널("Joints" 탭)에서 관절을 움직여 원하는 자세를 만든 뒤 tcp_link 좌표 읽기.
2. 시뮬레이션에서 로봇을 움직인 뒤 TF로 직접 측정: ros2 run tf2_ros tf2_echo world tcp_link
또한 툴 중심점 지정 누락에 주의해야 한다. 목표 좌표는 '물체의 위치'가 아니라 '로봇의 특정 링크'를 기준으로 정렬된다.
move_group_interface.setEndEffectorLink("tcp_link");이를 빼먹으면 기본값인 손목 끝단(tool0)이 목표 좌표로 이동하여 충돌이 발생한다.
3. INVALID_MOTION_PLAN: 그리퍼와 팔뚝 자기 충돌
증상
[INFO] [moveit_collision_detection_fcl]: Found a contact between 'forearm_link'
and 'finger2_link', which constitutes a collision.
[ERROR] [move_group]: PlanningResponseAdapter 'ValidateSolution' failed with error code INVALID_MOTION_PLAN원인
플래닝 최종 검증 단계에서 자기 충돌(Self-collision)이 감지되었다. 손목이 크게 꺾이면서 손가락(finger2_link)이 로봇 팔뚝(forearm_link)에 닿는 궤적이 생성된 것이다.
기존 ur_moveit_config의 SRDF 충돌 예외 목록(disable_collisions)은 그리퍼가 없는 팔 단독 모델을 기준으로 작성되었기 때문에, 새로 추가된 그리퍼 링크에 대한 검사 규칙이 없다.
이동 경로 추가
실제 물리적 충돌 가능성이기 때문에 disable_collisions에 두 링크를 수동으로 예외 처리하는 것은 바람직한 해결책이 아니다. 일단 움직이는 위치를 잡을 대상의 z축 바로 위로 먼저 이동하는 방식으로 경로를 추가하여 로봇 팔의 자세가 좀 더 안정적일 수 있도록 하는 것으로 처리하였다.
4. TIMED_OUT: IK(역운동학) 알고리즘의 수렴 실패
증상
동일한 시작/목표 지점임에도 성공과 실패가 무작위로 번갈아 발생한다.
시간(setPlanningTime)과 시도 횟수(setNumPlanningAttempts)를 크게 늘려도 0.0005초 만에 즉시 실패하는 로그가 반복된다.
[WARN] [ParallelPlan.cpp:138]: Unable to find solution by any of the threads in 0.000516 seconds
[ERROR] [move_group]: Planner 'OMPL' failed with error code TIMED_OUT경로를 찾지 못한다는 점만 가지고 경로 탐색 엔진을 바꾸 보는 방향으로 진행하였다.
TRAC-IK로 경로 탐색 알로리즘 교체
실패 확률을 낮추기 위해 병렬 연산이 적용된 TRAC-IK를 설치(ros-jazzy-trac-ik-kinematics-plugin)했다. 하지만 이 경로 계획 엔진도 실패 확률이 높기는 마찬가지 였다.
작업 시작점의 자세 변경:
이것 저것 실패 원인에 대한 정보를 찾아 보던 중에 로봇의 자세가 역운동학의 해를 찾기 힘든 원인이 될 수 있다는 정보를 얻었다.
살펴보니 지금까지의 작업 시작점이었던 UR 로봇의 home 자세는 역운동학(IK) 해를 구하기 까다로운 특이점(Singularity)에 해당하는 위치였다 (참고 1, 참고 2). 시작 자세를 test_position으로 변경하고 테스트하니 알고리즘 변경 없이도 반복 테스트 하였을때 목표 지점으로의 경로 계획이 잘 이루어 졌다.
