Chapter 5: 전통적 플래닝과 제어 — IK, MoveIt 2, ros2_control

개요

“그리퍼를 앞으로 10 cm 움직여라”는 한 문장처럼 들린다. 실제 stack은 여러 질문을 차례로 풀어야 한다. 어떤 관절 자세가 tool을 그 위치에 놓는가? 여러 IK 해 중 joint limit와 collision을 피하는 것은 무엇인가? 경로를 velocity·acceleration·jerk 한계 안에서 어떻게 time-parameterize할 것인가? 어떤 controller가 command interface를 소유하는가? 실제 robot이 reference를 충분히 따라가는가? 따라가지 못하면 누가 멈추고 어떤 evidence를 남기는가? Kinematics, planning, trajectory generation과 feedback control은 이 질문의 서로 다른 부분을 맡는다.

이 장은 두 목표를 끝까지 추적한다. Joint goal은 target configuration을 직접 정한다. Cartesian goal은 tool pose 또는 velocity를 정하므로 FK와 IK가 필요하다. MoveIt 2는 current state와 planning scene에서 collision-aware path를 찾고 trajectory processing이 시간을 붙인다. Trajectory controller는 timed reference를 hardware command로 바꾸며, vendor controller는 low-level servo와 safety state의 authority를 유지한다. Contact task에는 force 또는 impedance가 추가되지만 검증된 free-space position path를 대충 대신하지 않는다.

Raw controller code는 싣지 않는다. 대신 decision table, measurable gate, failure branch와 bounded Codex prompt를 제공한다. Software 상태는 2026-07-14 기준이다. 이 경로를 simulator에서 연습하는 법은 (Chapter 6), supervised first motion은 (Chapter 7)에서 이어진다.

이 장을 읽고 나면... - joint, Cartesian pose, Cartesian velocity goal을 구분하고 FK, IK, differential IK가 들어가는 위치를 설명할 수 있다. - MoveIt 2의 robot state, planning scene, planner, trajectory processing, execution 경계를 그릴 수 있다. - global planning, local Servo, position control, impedance/force control을 과제 조건에 맞게 선택할 수 있다. - singularity, collision, timing, controller, hardware tracking failure를 서로 다른 layer로 진단할 수 있다. - generated code에 motion/safety authority를 주지 않고 fake hardware에서 real hardware까지 단계별 검증할 수 있다.

1. 목표 표현부터 고른다

회전 관절 manipulator의 configuration을 q라고 하면 forward kinematics는 tool transform T=f(q)를 계산한다. Joint goal은 “q_g로 가라”고 하므로 actuator coordinate에서 명확하지만 operator가 이해하기 어렵다. Cartesian goal은 “TCP frame을 T_g에 놓아라”고 하므로 inverse kinematics가 f(q)=T_g를 만족하는 하나 이상의 q를 찾아야 한다. Redundancy, elbow posture, joint limit, collision과 seed 때문에 IK는 보통 하나의 정답이 아니다.

Sign과 limit commissioning, named home, calibration pose에는 joint goal부터 쓴다. Pick/place, approach와 tool alignment에는 Cartesian pose가 자연스럽다. Teleoperation, visual correction과 작은 정렬에는 Cartesian velocity 또는 incremental pose가 맞다. Local velocity method에게 큰 obstacle을 돌아가는 일을 맡기지 않고, 1 mm correction마다 global planner를 부르지도 않는다.

그림 5.1 — 두 goal representation이 하나의 실행 계약으로 합류하는 end-to-end pipeline. 원본 개념도.

2. FK, IK와 differential IK를 구분한다

Geometric Jacobian J(q)는 joint velocity와 tool twist를 \dot{x}=J(q)\dot{q}로 연결한다. Singularity에서 멀고 square full-rank라면 inverse를 생각할 수 있고, redundant arm은 pseudoinverse와 null-space motion을 쓴다. Null-space objective는 primary Cartesian velocity를 유지하며 joint limit을 피하거나 posture를 선호할 수 있지만 constraint가 강해지면 objectives가 경쟁한다.

Differential IK는 Cartesian velocity를 Jacobian을 통해 mapping하며, singularity 근처에서는 ordinary inverse가 command를 증폭하므로 damping이 필요하다 ^{[1] [2]}. Rank가 줄어들면 작은 tool velocity가 큰 joint velocity를 요구한다. Damped least squares는 정확한 tracking 일부를 포기해 command를 bounded하게 만든다. Singularity를 없애는 것이 아니므로 margin이 소진되기 전에 scale, stop 또는 replan한다.

Numerical pose IK는 pose error를 반복적으로 줄이고 analytical IK는 특정 geometry에서 여러 branch를 열거한다. 둘 다 seed, limit, TCP와 frame convention이 필요하다. Pose tolerance를 만족해도 joint limit에 붙거나 collision할 수 있으므로 IK success는 candidate generation 결과이지 execution permission이 아니다.

한 숫자의 threshold만 믿지 않는다. Minimum singular value, condition number, commanded joint-speed amplification, joint-limit distance를 함께 기록한다. Orientation constraint 때문에 position은 가능해도 practical singularity가 생길 수 있다. Threshold는 arm과 task에서 실제 deceleration/stop behavior로 검증한다.

3. 충돌 없는 path는 중간 결과일 뿐이다

MoveIt move_group은 current state, URDF/SRDF, kinematics, planning scene, constraints, planner pipeline, trajectory processing과 controller execution을 묶는다 ^[4]. Planning scene에는 robot link, self-collision rule, installed tool, attached object, table, fixture와 relevant world geometry가 있어야 한다. Visual mesh가 좋은 collision mesh라는 보장은 없고, 누락된 cable이나 camera mast는 현실에서 사라지지 않는다.

Collision-free path에도 valid start state, joint limits, time parameterization, controller compatibility와 current-state monitoring이 필요하다 ^{[3] [4] [5]}. Stale joint state에서 계획하면 첫 waypoint에서 discontinuity가 생긴다. Geometric waypoint에는 timestamp가 없고 controller는 joint list, tolerance 또는 interface가 다르면 path가 좋아도 reject한다.

Planning scene update policy도 결정한다. 움직이는 object estimate가 planning과 execution 사이에 달라지면 stop, replan 또는 monitored execution 중 하나를 명시한다. Attached-object geometry와 allowed-collision entry에는 lifetime이 있어야 한다. 놓은 object가 software상 계속 attached된 상태는 다음 planning을 오염시킨다.

RRT-Connect, trajectory optimization과 constrained planning은 서로 다른 구조를 풀기 때문에 어느 benchmark도 하나의 universal planner를 정당화하지 않는다 ^{[7] [8] [9] [10]}. Sampling planner는 복잡한 공간에서 path를 찾기 좋지만 seed variation이 있고, optimization은 smooth local improvement에 강하지만 나쁜 homotopy에 갇힐 수 있다. Upright cup이나 surface-following은 constraint manifold가 핵심이다.

Planner pipeline, parameters, random seed, planning time, path length, clearance와 failure reason을 저장한다. Contact mode까지 planning해야 할 때는 Terry의 한국어 global planning 글과 영어 글을 참고할 수 있다. 첫 motion에서는 free-space planning과 contact execution을 먼저 분리한다.

4. Time parameterization이 path를 trajectory로 만든다

Path는 configuration 순서를 정하고 trajectory는 time, velocity와 acceleration을 붙인다. TOPP-RA는 path constraint 아래 parameterization을, Ruckig는 online jerk-constrained generation을 다룬다 ^{[11] [12]}. 서로 다른 input과 guarantee를 가진다.

Timestep과 start-at-rest 같은 MoveIt trajectory processing assumption은 robot controller와 맞는지 확인해야 한다 ^{[5] [11] [12]}. 특정 branch의 recommendation은 jerk나 torque compatibility를 보장하지 않는다. 첫 point의 zero velocity 전제, strictly increasing timestamp, controller가 velocity/acceleration field를 실제 소비하는지 확인한다.

Slow scaling만으로 나쁜 trajectory가 좋아지지 않는다. Duration을 늘리면 velocity와 acceleration은 줄 수 있지만 curvature discontinuity나 interpolation mismatch는 남는다. Commanded position/velocity/acceleration과 actual state를 함께 본다. 각 limit가 URDF, joint-limits YAML, vendor controller 또는 application override 중 어디서 왔는지 기록한다.

실행 전 gate

1. Start state age가 cell threshold 안이고 실제 robot과 일치한다.
2. Controller order로 모든 joint가 한 번씩 있고 timestamp가 증가한다.
3. Position, velocity, acceleration, jerk limit의 근거가 있다.
4. Installed tool과 attached object를 포함한 clearance가 충분하다.
5. Start/end velocity assumption이 controller behavior와 맞는다.
6. Duration과 joint delta가 reduced first-motion envelope 안이다.
7. Cancel, tolerance violation과 hold를 fake hardware에서 검증했다.

5. ros2_control과 Joint Trajectory Controller의 실행 계약

ros2_control은 robot-specific read/write를 hardware interface로 분리하고 reusable controller가 command/state interface를 사용하게 한다 ^[13]. Controller Manager는 update cycle과 ownership을 맡는다. Joint Trajectory Controller는 time-parameterized reference를 받고 configured path/goal tolerance로 feedback을 비교한다 ^[14]. 이는 MoveIt 아래, vendor servo 위의 contract다.

Joint Trajectory Controller는 configured tolerance를 위반하면 action을 abort한다 ^[14]. Abort는 귀찮은 장애가 아니라 evidence다. Joint, error, tolerance, stamp, controller state와 hardware fault를 보존한다. Hold behavior는 valid feedback과 hardware implementation에 달리므로 안전하게 고정된다고 가정하지 않고 시험한다.

Controller update rate, missed cycle과 command-limit enforcement는 모든 hardware test에서 logging해야 한다 ^[15]. Mean rate는 드문 overrun을 숨긴다. Worst period, missed-cycle diagnostic, state age, clamp와 CPU/load context를 남긴다. Software limit enforcement는 defense in depth이지 safety-rated limit가 아니다.

그림 5.2 — Planning success와 execution success 사이의 책임·실패·증거 경계. 원본 개념도.

6. Vendor controller가 실제 execution을 바꾼다

Vendor controller는 interpolation, servo, mode와 protective state의 authority다. 같은 ROS trajectory도 UR와 Franka에서 command contract와 inner loop가 달라 다르게 보인다. Gain, tolerance와 speed assumption을 platform 사이에 복사하지 않는다.

Universal Robots speed scaling은 trajectory progress를 바꾸므로 wall-clock execution이 nominal planned duration과 달라질 수 있다 ^[16]. Driver가 관련 scaling factor를 반영하고 slider나 safety scaling이 progress를 늦춘다. Timestamp를 줄이거나 gain을 높이기 전에 scaling을 진단한다. Nominal/actual duration, scaling trace, safety state와 result를 기록한다.

Franka torque/impedance 연구에서는 real-time host, libfranka tuple, payload model, collision threshold와 vendor example이 시작 boundary다. Simulation에서 도는 controller도 delayed host에서 overrun 또는 instability가 생길 수 있다. Custom torque 전에는 vendor position/trajectory mode를 먼저 통과한다.

7. MoveIt Servo는 local motion이지 global planner가 아니다

MoveIt Servo는 streaming joint/Cartesian command를 bounded local motion으로 바꾼다. Joystick/VR teleoperation, visual correction과 incremental alignment에 유용하지만 큰 obstacle을 돌아가는 route를 찾지 않는다. Clutch, workspace, stale-command timeout과 frame contract는 application 책임이다.

MoveIt Servo에는 valid robot model, fast feedback, compatible controller, singularity scaling과 collision scaling이 필요하다 ^{[6] [2]}. Scaling은 safety proof가 아니다. Approach direction, command frame, timeout, collision deceleration, singularity retreat와 controller latency를 simulation과 reduced motion으로 시험한다.

실용적인 hybrid는 global plan으로 pre-grasp까지 간 뒤 마지막 몇 cm를 Servo로 수행한다. Switching할 때 explicit stop, ownership와 current-state refresh가 필요하다. Global action과 Servo stream이 같은 interface를 동시에 command하면 안 된다.

8. Position, velocity, torque, impedance와 force control

Position은 joint 위치, velocity는 속도, torque는 effort를 command한다. Impedance는 disturbance에 대해 exact path만 강제하지 않고 motion error와 interaction wrench의 관계를 만든다 ^[17]. Hybrid position/force는 surface가 일부 방향을 구속할 때 motion과 force regulation 방향을 나눈다 ^[18]. Operational-space formulation은 Cartesian objective와 dynamics를 연결한다 ^[19].

Realized impedance는 model accuracy, inner-loop bandwidth, delay, saturation과 environment stiffness에 달린다 ^{[17] [20] [21]}. “500 N/m” 입력이 measured tool behavior를 보장하지 않는다. Robot과 environment가 한 closed loop를 이루고 stiff environment와 delay가 energy를 주입할 수 있다. Compliant fixture, measured wrench, bounded speed와 independent stop으로 낮은 energy부터 검증한다.

Free-space pick/place에는 position control이 적절하다. Contact를 감지하고 멈추는 정도면 guarded velocity/position approach가 충분할 수 있다. Compliance가 필요하면 impedance, measured component regulation이 필요하면 force/hybrid를 고려한다. 모든 contact task가 raw torque access를 요구하는 것은 아니며 vendor impedance/force mode가 더 검증 가능할 수 있다.

DexForce는 여섯 contact-rich task에서 평균 76 percent success를 보고했고 pose-only action은 near zero였다 ^[22]. 이는 해당 hardware, task, compliance tuning과 force reconstruction에 묶인 결과다. Force-informed representation이 중요할 수 있다는 근거이지 classical controller를 대체한다는 뜻이 아니다. Terry의 관련 글은 한국어와 영어로 연결된다.

그림 5.3 — Free space에서 contact로 넘어갈 때 바뀌는 control contract와 독립 정지 경계. 원본 개념도.

9. 두 목표를 끝까지 추적한다

Joint goal walkthrough

한 joint, 작은 signed delta, reduced speed와 collision-free scene을 고른다. Fresh state를 읽고 soft/vendor limit 안 goal을 만든다. Conservative duration으로 time parameterize하고 action을 보낸다. Accept, feedback, actual error, scaling과 result를 관찰한다. Fake hardware에서 halfway cancel과 hold를 먼저 검증한다. 실물에서는 observer가 stop을 소유하고 wrong direction, excess error, stale state 또는 unexpected mode에서 끝낸다.

Cartesian goal walkthrough

TCP를 정의하고 known timestamp의 base frame으로 target을 표현한다. Controlled seed에서 여러 IK candidate를 만든다. Limit/collision margin이 나쁜 후보를 버린다. Current state와 scene에서 plan하고 timing한 뒤 clearance와 joint delta를 검사한다. Reduced path를 실행하면서 IK branch와 configuration hash를 저장한다. Controller completion을 Cartesian accuracy로 간주하지 말고 실제 final TCP를 독립 측정한다.

10. Fake hardware에서 현실까지의 runbook

1. Static model: axis, limit, TCP, self-collision, tool와 fixture geometry를 확인한다.
2. IK-only: workspace target과 seed를 sweep해 unreachable/limit/singular region을 그린다.
3. Planning-only: known start, obstacle, attached object와 repeated seed를 시험한다.
4. Fake controller: accept, feedback, cancel, tolerance abort, hold와 ownership을 시험한다.
5. Physics simulation: command/state를 비교하고 delay, scaling과 collision을 주입한다.
6. Drives disabled: real state freshness, controller list, mode, limits와 time을 확인한다.
7. One joint: reduced positive/negative delta를 observer 아래 실행한다.
8. Short joint trajectory: conservative timing, cancel과 repeatability를 본다.
9. Free-space Cartesian: 큰 clearance와 independent pose measurement를 쓴다.
10. Guarded contact: free-space gate 뒤 승인된 낮은 energy/force limit에서만 한다.

Simulation success를 이유로 learned/Cartesian policy로 건너뛰지 않는다. 각 단계가 다른 ambiguity를 제거한다. Command, state, scene, planner, controller, vendor와 safety trace를 하나의 attempt ID로 저장한다.

11. Bounded Codex prompt

Prompt A — pipeline audit

Robot description, MoveIt config, ros2_control config, controller list와 공식 vendor
문서만 읽어라. 파일을 수정하거나 hardware에 접속하지 말라. Named joint goal 하나와
Cartesian goal 하나를 frame, IK, planning scene, planner, time parameterization,
FollowJointTrajectory, hardware interface, vendor controller까지 추적하라.

각 boundary의 unit, timestamp, limits, joints, owner, failure result를 표로 만들라.
근거 없는 assumption은 UNKNOWN으로 두고 blocker, fake-hardware test, real motion 전
필요 evidence를 반환하라. Limit/safety bypass를 제안하지 말라.

Prompt B — simulator-only test

Repository convention을 따라 simulator/fake-hardware test만 구현하라. 작은 joint delta와
collision-free Cartesian target 하나를 쓰라. Fresh state, valid TF, start match, limits,
monotonic timestamps, one command owner, feedback/result, cancel, tolerance abort를 assert하라.

Stale state, wrong frame, collision, controller rejection에서 test가 실패하고 diff와 명령을
human reviewer가 읽을 수 있을 때 완료다. Real IP, enable, torque, auto retry, safety-limit
변경을 넣지 말라.

Prompt C — offline diagnosis

Offline planner/action/controller/state/timing/scaling/vendor logs에서 failure를 goal/TF,
IK, collision/path, timing, controller interface, tracking, vendor mode, payload 또는
contact stability로 분류하라. Evidence로 hypothesis를 rank하고 read-only 또는 simulated
falsification test를 하나씩 제시하라.

Observed facts, missing signal, safest next test, stop condition, human owner를 반환하라.
독립 승인 없이 tolerance/stiffness/force/speed 증가, collision/watchdog 해제를 권하지 말라.

Codex는 documented API와 test를 compose할 수 있지만 feedback controller나 safety authority가 아니다. 관련 CaP-X 논의는 한국어와 영어에서 볼 수 있다.

12. 근거 등급, 논쟁점과 한계

MoveIt과 ros2_control behavior는 current 공식 documentation을 썼다. IK, planning, timing과 impedance는 primary academic sources를 썼다. Vendor scaling 문서는 UR-specific behavior의 근거이지 arm ranking이 아니다. Benchmark는 robot, scene, parameters, controller와 metric에 묶인다.

Global planning 대 local Servo는 조건부 선택이다. Global planner는 큰 collision-aware route에 적합하지만 scene과 planning latency가 필요하다. Servo는 local input에 빠르지만 global route를 보장하지 않고 collision/singularity에서 scale해야 한다. Hybrid가 흔히 맞으며 어느 쪽도 universal replacement가 아니다.

Position 대 impedance도 조건부다. Free space에서 position은 단순하고 재현 가능하다. Impedance는 contact uncertainty를 흡수하지만 model error, delay, saturation과 environment stiffness라는 stability risk를 산다. “Compliant”라는 label 대신 energy, wrench, tracking과 stop behavior를 측정한다.

한계도 남는다. Software/vendor behavior는 2026-07-14 이후 바뀐다. Geometric simulation은 real tracking/contact를 검증하지 않는다. Trajectory documentation은 torque/jerk compatibility를 자동 보장하지 않는다. Safety authority는 실제 controller, cell assessment와 qualified review에 남는다.

제조 셀 적용 체크포인트

Tray-to-fixture pick에 joint home, Cartesian pre-grasp, straight guarded approach, grasp, retreat와 placement를 정의한다. Planning scene owner는 table, fixture, tool와 attached object를 관리하고 TCP/calibration revision을 기록한다. Free-space는 global planning, 마지막 contact는 별도 검증한 local/contact method를 쓴다.

Log에는 attempt_id, goal_type, frame/stamp, start_state_age, ik_plugin/seed/solution, scene_hash, planner/seed, clearance, timing_method, controller, tolerances, update/missed_cycles, speed_scaling, action_result, vendor_fault, measured_pose, intervention을 넣는다. KPI는 seed별 planning success, clearance, tracking error, abort/cancel latency, final pose error, contact peak와 recovery time이다.

다음에 배울 것

이제 kinematics가 candidate를 만들고, planning이 collision-free path를 고르며, timing이 trajectory를 만들고, controller가 reference를 실행하며, feedback이 차이를 드러내는 전체 contract를 보았다. Chapter 6에서는 Isaac Sim, Isaac Lab과 MuJoCo가 이 contract의 어느 부분을 검증하고 학습하는지, 같은 robot/task를 simulator 사이에서 어떻게 비교하는지 설명한다.

넘어가기 전에 자신의 cell에서 joint goal 하나와 Cartesian goal 하나를 택해 이 장의 표를 모두 채운다. Frame, limit, interpolation, controller owner 또는 stop behavior가 UNKNOWN이면 실물 motion 전에 fake hardware나 공식 문서에서 닫는다.

주석이 있는 연구 경로

아래 자료는 planning, control과 contact reasoning를 더 깊이 확인하기 위한 경로다. 성공률을 빌려오는 목록이 아니라, 어떤 가정·interface·실험을 원문에서 비교해야 하는지에 따라 묶었다. 각 결과는 해당 platform과 protocol에 한정해 읽는다.

참고문헌

1. Whitney, D. E. (1969). Resolved Motion Rate Control of Manipulators and Human Prostheses. IEEE Transactions on Man-Machine Systems. DOI: 10.1109/TMMS.1969.299896.
2. Nakamura, Y., & Hanafusa, H. (1986). Singularity-Robust Inverse Kinematics. Journal of Dynamic Systems, Measurement, and Control. DOI: 10.1115/1.3143764. [Nakamura & Hanafusa, 1986]
3. Șucan, I. A., et al. (2012). The Open Motion Planning Library. IEEE Robotics & Automation Magazine. DOI: 10.1109/MRA.2012.2205651.
4. MoveIt (2025a). MoveIt 2 move_group Architecture. Official documentation.
5. MoveIt (2025b). MoveIt 2 Trajectory Processing. Official documentation.
6. MoveIt (2025c). MoveIt Servo Realtime Teleoperation Tutorial. Official documentation.
7. Kuffner, J. J., & LaValle, S. M. (2000). RRT-Connect: An Efficient Approach to Single-Query Path Planning. IEEE ICRA. DOI: 10.1109/ROBOT.2000.844730. [Kuffner & LaValle, 2000]
8. Ratliff, N., et al. (2009). CHOMP: Gradient Optimization Techniques for Efficient Motion Planning. IEEE ICRA. DOI: 10.1109/ROBOT.2009.5152817.
9. Schulman, J., et al. (2014). Motion Planning with Sequential Convex Optimization and Convex Collision Checking. IJRR. DOI: 10.1177/0278364914528132.
10. Berenson, D., et al. (2009). Manipulation Planning on Constraint Manifolds. IEEE ICRA. DOI: 10.1109/ROBOT.2009.5152399.
11. Pham, H., & Pham, Q.-C. (2018). TOPP-RA. IEEE Transactions on Robotics. DOI: 10.1109/TRO.2018.2821155.
12. Berscheid, L., & Kröger, T. (2021). Ruckig. Robotics: Science and Systems. DOI: 10.15607/RSS.2021.XVII.015.
13. Chitta, S., et al. (2017). ros_control. JOSS. DOI: 10.21105/joss.00456.
14. ros-controls (2026a). Joint Trajectory Controller — Jazzy. Official documentation.
15. ros-controls (2026b). Controller Manager Monitoring and Limits. Official documentation.
16. Universal Robots (2026). UR ROS 2 Controllers and Speed Scaling. Official documentation.
17. Hogan, N. (1985). Impedance Control, Part I. Journal of Dynamic Systems, Measurement, and Control. DOI: 10.1115/1.3140702.
18. Raibert, M. H., & Craig, J. J. (1981). Hybrid Position/Force Control of Manipulators. DOI: 10.1115/1.3139652.
19. Khatib, O. (1987). A Unified Approach for Motion and Force Control. DOI: 10.1109/JRA.1987.1087068.
20. Ott, C., et al. (2008). Cartesian Impedance Control. IEEE ICRA. DOI: 10.1109/ROBOT.2008.4543616.
21. Casini, D., et al. (2019). Response-Time Analysis of ROS 2 Processing Chains. DOI: 10.4230/LIPIcs.ECRTS.2019.6.
22. Chen, C., et al. (2025). DexForce. arXiv:2501.10356.