Chapter 5: Classical Planning and Control — IK, MoveIt 2, and ros2_control

Overview

“Move the gripper ten centimeters forward” sounds like one command. A robot stack must answer several different questions: Which joint configurations place the tool there? Which solution avoids joint limits and collisions? How should the path be timed within velocity, acceleration, and jerk limits? Which controller owns the command interfaces? Does the real robot follow closely enough, and what happens when it does not? Planning, trajectory generation, and feedback control solve different parts of this chain.

This chapter follows two goals end to end. A joint goal directly specifies a target configuration. A Cartesian goal specifies a tool pose or velocity and requires forward/inverse kinematics. MoveIt 2 builds a collision-aware path from the current state and planning scene, then trajectory processing assigns time. A trajectory controller converts those timed references into hardware commands; the vendor controller retains the authoritative low-level servo and safety state. Contact tasks add force or impedance behavior, but not as a drop-in replacement for a validated position path.

No raw controller code is required here. The tutorial instead gives decision tables, measurable gates, failure branches, and bounded Codex prompts. Software and product status is current to 2026-07-14. The simulator roles needed to rehearse this path follow in (Chapter 6), and the supervised first-motion sequence follows in (Chapter 7).

After reading this chapter... - You can distinguish joint, Cartesian pose, and Cartesian velocity goals and identify where FK, IK, and differential IK enter. - You can explain MoveIt 2's robot state, planning scene, planner, trajectory processing, and execution boundaries. - You can select global planning, local Servo, position control, or impedance/force control from task assumptions. - You can diagnose singularity, collision, timing, controller, and hardware-tracking failures as separate layers. - You can run a staged fake-to-real validation without delegating motion or safety authority to generated code.

1. Start with the goal representation

A revolute manipulator configuration is a joint vector q. Forward kinematics maps it to a tool transform T=f(q). A joint goal says “reach q_g”; it is unambiguous at the actuator-coordinate level but may be unintuitive to an operator. A Cartesian goal says “place TCP frame at T_g”; inverse kinematics must find one or more q satisfying f(q)=T_g. Redundancy, multiple elbow postures, limits, collision, and seed choice mean that IK is rarely a single universal answer.

Use a joint goal first when commissioning signs and limits, moving to a named home configuration, or reproducing a known calibration pose. Use a Cartesian pose goal for pick/place, approach, and tool alignment. Use a Cartesian velocity or incremental pose for teleoperation, visual servoing, and small corrections. Do not ask a local velocity method to solve a large obstacle-filled move; do not invoke a global planner for every millimeter of continuous teleoperation.

Figure 5.1 — Two goal representations converging on one end-to-end execution contract. Original synthesis.

2. FK, IK, and differential IK are related but not interchangeable

The geometric Jacobian J(q) relates joint velocity to tool twist: \dot{x}=J(q)\dot{q}. Away from singularity, a square full-rank case suggests \dot{q}=J^{-1}\dot{x}; redundant arms use a pseudoinverse plus optional null-space motion. A null-space objective can avoid joint limits or prefer posture without changing the primary Cartesian velocity, but secondary objectives compete when constraints tighten.

Differential IK maps Cartesian velocity through the Jacobian, and damping is needed because an ordinary inverse amplifies commands near singularities. ^{[1] [2]} Near rank loss, a small desired tool motion can demand very large joint velocity. Damped least squares trades exact Cartesian tracking for bounded behavior. It does not make the singularity disappear; scale, stop, or replan before the margin is exhausted.

Numerical pose IK iteratively reduces pose error; analytical IK may enumerate branches for a specific geometry. Both need seed, joint limits, TCP, and frame conventions. A solution can satisfy pose tolerance while grazing a limit or colliding. Therefore IK success is a candidate-generation result, not permission to execute.

Track singularity with more than one magic threshold. Useful signals include the smallest singular value, condition number, commanded joint-speed amplification, and distance to limits. Validate the chosen measure against the actual arm and task. Orientation constraints can create a practical singularity even when position alone remains feasible.

IK troubleshooting

3. A collision-free path is only the geometric middle

MoveIt move_group assembles current robot state, URDF/SRDF, kinematics, planning scene, constraints, planner pipeline, trajectory processing, and controller execution ^[4]. The planning scene must contain robot links, self-collision rules, attached tools/objects, table, fixtures, and relevant world geometry. A visual mesh is not necessarily an appropriate collision mesh; a missing cable or camera mast remains a real hazard.

A collision-free path still needs a valid start state, joint limits, time parameterization, controller compatibility, and current-state monitoring. ^{[3] [4] [5]} A path planned from stale joints can begin with a discontinuity. A geometric waypoint sequence has no executable rate until timestamps are assigned. A controller may reject joints, tolerances, or interfaces even when the path is valid.

The planning-scene update policy matters. A moving object estimate can invalidate the scene between planning and execution. Decide whether to stop, replan, or monitor the environment during execution; “planned once” is not collision monitoring. Attached-object geometry and allowed-collision entries must have explicit lifetimes, or a released object can remain attached in software.

RRT-Connect, trajectory optimization, and constrained planning solve different structures, so no benchmark justifies one universal planner. ^{[7] [8] [9] [10]} Sampling-based planning is strong at finding paths through complex spaces but can vary by seed. Trajectory optimization exploits smoothness and local gradients but can settle in a poor homotopy. Constraint-manifold planning is appropriate when a cup must remain upright or a tool must follow a surface. Record pipeline, parameters, seed, planning time, path length, clearance, and failure reason.

Related Terry notes on global planning for contact-rich manipulation in Korean and English are useful when contact mode becomes part of planning. For first motion, separate free-space planning from contact execution before combining them.

4. Time parameterization turns a path into a trajectory

A geometric path orders configurations; a trajectory assigns time, velocity, and possibly acceleration. Time parameterization must respect joint limits and the assumptions of the downstream controller. TOPP-RA treats path parameterization under constraints, while Ruckig focuses on online jerk-constrained trajectory generation ^{[11] [12]}. These methods address different inputs and guarantees.

MoveIt trajectory processing assumptions such as timestep and start-at-rest must be checked against the robot controller. ^{[5] [11] [12]} A branch recommendation does not guarantee jerk, torque, or vendor interpolation compatibility. Confirm whether the first point assumes zero velocity, whether timestamps increase strictly, and whether the controller consumes velocity/acceleration fields or reconstructs them.

Slow scaling is useful but not sufficient. Scaling trajectory duration may reduce velocity and acceleration while leaving poor curvature, discontinuity, or controller mismatch. Compare commanded position, velocity, acceleration, and actual state. Record the exact limit source: URDF, joint-limits YAML, vendor controller, or application override.

Pre-execution trajectory gate

1. Start state age is within the cell threshold and matches the real robot.
2. Every joint appears exactly once in controller order with continuous timestamps.
3. Position, velocity, acceleration, and jerk limits use documented sources.
4. Minimum collision clearance includes the installed tool and attached object.
5. First/last velocity assumptions match controller behavior.
6. Duration and maximum joint delta fit the reduced first-motion envelope.
7. Cancel, tolerance violation, and hold behavior have passed fake-hardware tests.

5. ros2_control and Joint Trajectory Controller execute a contract

ros2_control separates hardware read/write interfaces from reusable controllers ^[13]. Controller Manager owns the update cycle and command-interface arbitration. Joint Trajectory Controller accepts time-parameterized references and compares feedback against configured path and goal tolerances ^[14]. This contract is above the vendor servo and below MoveIt.

Joint Trajectory Controller aborts an action when configured tolerances are violated. ^[14] An abort is evidence, not merely inconvenience. Preserve joint, error, tolerance, timestamp, controller state, and hardware fault. Holding behavior depends on valid feedback and hardware implementation; test it rather than assuming a safe freeze.

Controller update rate, missed cycles, and command-limit enforcement should be logged in every hardware test. ^[15] Mean rate hides rare overruns. Keep worst observed period, missed-cycle diagnostic, state age, command clamp, and CPU/load context. Software limit enforcement is defense in depth, not a safety-rated limit.

Figure 5.2 — Responsibility, failure, and evidence boundaries between planner success and execution success. Original synthesis.

6. Vendor execution changes the observed trajectory

The vendor controller remains authoritative for internal interpolation, servo, mode, and protective state. The same ROS trajectory can behave differently on UR and Franka because their command contracts and inner loops differ. Do not copy gains, tolerance, or speed assumptions across platforms.

Universal Robots speed scaling changes trajectory progress, so wall-clock execution can diverge from nominal planned duration. ^[16] The driver combines relevant scaling factors; reduced slider or safety scaling changes progress. Diagnose scaling before shortening timestamps or raising gains. Log nominal duration, actual duration, scaling trace, safety state, and action result.

For Franka torque or impedance work, the real-time host, libfranka/version tuple, payload model, collision thresholds, and vendor examples form the starting boundary. A controller that runs in simulation may overrun or become unstable on a delayed host. Begin with vendor position/trajectory modes before introducing custom torque.

7. MoveIt Servo is local motion, not a global planner

MoveIt Servo maps streaming joint or Cartesian commands into bounded local motion. It is useful for joystick/VR teleoperation, visual correction, and incremental alignment. It does not discover a route around a large obstacle like a global planner. A clutch, workspace boundary, stale-command timeout, and frame contract remain application responsibilities.

MoveIt Servo requires valid robot models, fast feedback, a compatible controller, and singularity and collision scaling. ^{[6] [2]} Scaling is not proof of safety. Test approach direction, frame, command timeout, collision deceleration, singularity retreat, and controller latency in simulation and reduced hardware motion.

A practical hybrid globally plans to a pre-grasp, then uses Servo for the last centimeters under perception or operator input. Switching requires an explicit stop, controller ownership check, and current-state refresh. Never let the global action and Servo stream command the same interface concurrently.

8. Position, velocity, torque, impedance, and force control answer different questions

Position control asks where the joint should be; velocity control asks how fast; torque control commands effort subject to the platform contract. Impedance control shapes a relationship between motion error and interaction wrench rather than demanding an exact path against every disturbance ^[17]. Hybrid position/force control selects directions for motion and force regulation, useful when a surface constrains part of the task ^[18]. Operational-space formulations relate Cartesian objectives to robot dynamics ^[19].

Realized impedance depends on model accuracy, inner-loop bandwidth, delay, saturation, and environment stiffness. ^{[17] [20] [21]} Entering “500 N/m” does not guarantee that measured tool behavior equals that number. Robot and environment form one closed loop; a stiff environment and delayed controller can inject energy. Validate low-energy contact with compliant fixtures, measured wrench, bounded speed, and an independent stop.

Use position control for free-space pick/place when contact uncertainty is small. Use guarded velocity/position approaches when detecting contact and stopping is enough. Consider impedance when contact must remain compliant, and force/hybrid control when a measured contact component must be regulated. Torque access is not automatically necessary for every contact task; vendor impedance or force modes may be safer and easier to validate.

DexForce reports 76 percent average success over six contact-rich tasks while pose-only actions were near zero. ^[22] This result is specific to its hardware, tasks, compliance tuning, and force reconstruction. It shows that action representation can matter, not that force-informed learning replaces classical control. Terry's related note is available in Korean and English.

Figure 5.3 — The control contract and independent-stop boundary change when free-space motion enters contact. Original synthesis.

9. Trace two complete goals

Joint-goal walkthrough

Choose one joint, a small signed delta, reduced speed, and a collision-free static scene. Read fresh joint state. Form a goal within soft and vendor limits. Time-parameterize with a conservative duration. Send through the trajectory action. Observe acceptance, feedback, actual error, scaling, and result. Cancel halfway in fake hardware first, then validate hold. On real hardware, an observer owns the stop and the attempt ends on wrong direction, excess error, stale state, or unexpected mode.

Cartesian-goal walkthrough

Define the TCP and express the target in base at a known timestamp. Generate several IK candidates from controlled seeds. Reject candidates near limits or collision. Plan from current state through the scene, time-parameterize, and inspect clearance and joint deltas. Execute the reduced path; log the exact IK branch and configuration hashes. Measure the real final TCP independently instead of equating controller completion with Cartesian accuracy.

10. Staged fake-to-real runbook

1. Static model: verify axes, limits, TCP, self-collision, tool and fixture geometry.
2. IK-only: sweep workspace targets and seeds; map unreachable, limit, and singular regions.
3. Planning-only: test known start states, obstacles, attached objects, and repeated seeds.
4. Fake controller: test action accept, feedback, cancel, tolerance abort, hold, and ownership.
5. Physics simulation: compare commanded and simulated state; inject delay, scaling, and collision.
6. Drives-disabled connection: verify real state freshness, controller list, mode, limits, and time.
7. One-joint reduced motion: positive and negative deltas under observation.
8. Short joint trajectory: conservative timing, cancel test, and repeatability.
9. Free-space Cartesian path: large clearance and independent final-pose measurement.
10. Guarded contact: only after free-space gates, with low energy and approved force/stop limits.

Do not skip directly to a learned or Cartesian policy because it “worked in simulation.” Each stage removes a different ambiguity. Save command, state, scene, planner, controller, vendor, and safety traces under one attempt ID.

Troubleshooting matrix

11. Bounded Codex prompts

Prompt A — planning-pipeline audit

Read this robot description, MoveIt config, ros2_control config, controller list, and
official vendor docs. Do not modify files or connect to hardware. Trace one named joint
goal and one Cartesian goal through frames, IK plugin, planning scene, pipeline, time
parameterization, FollowJointTrajectory, hardware interface, and vendor controller.

For every boundary report units, timestamps, limits, joints, owner, and failure result.
Mark unsupported assumptions UNKNOWN. Return a table of blockers, a fake-hardware test
plan, and evidence needed before real motion. Do not suggest limit/safety bypasses.

Prompt B — safe simulator test artifact

Implement only a simulator/fake-hardware test following repository conventions.
Use a small joint delta and one collision-free Cartesian target. Assert fresh state,
valid TF, start-state match, joint limits, monotonic trajectory timestamps, one command
owner, action feedback/result, cancel, and tolerance-abort behavior.

Done when tests fail for stale state, wrong frame, collision, and controller rejection;
the diff and commands are human-reviewable. Never include a real robot IP, enable step,
torque command, automatic retry loop, or safety-limit change.

Prompt C — controller-log diagnosis

Given offline planner, action, controller, joint-state, timing, scaling, and vendor-fault
logs, classify failure as goal/TF, IK, collision/path, timing, controller interface,
tracking, vendor mode, payload, or contact stability. Rank hypotheses by evidence and
give one read-only or simulated falsification test for each.

Return observed facts, missing signals, safest next test, stop condition, and human owner.
Do not recommend larger tolerance, stiffness, force, speed, disabled collision checking,
or watchdog removal without an independently approved engineering basis.

Codex may compose documented APIs and tests, but it must not become the feedback controller or safety authority. The related CaP-X discussion is available in Korean and English.

12. Evidence tiers, disagreements, and limitations

MoveIt and ros2_control behavior uses current official documentation. Classical IK, planning, timing, and impedance arguments use primary academic sources. Vendor scaling documentation supports UR-specific behavior, not an independent arm ranking. A paper's benchmark remains bound to robot, scene, planner parameters, controller, and metric.

Global planning versus local Servo is conditional. Global planning handles large collision-aware routes but assumes a scene and incurs planning delay. Servo responds quickly to local commands but cannot promise a global route and must scale near collision and singularity. A hybrid is often appropriate; neither universally replaces the other.

Position versus impedance is also conditional. Position control is simple and repeatable in free space. Impedance can tolerate contact uncertainty, but model error, delay, saturation, and environment stiffness create new stability risks. The label “compliant” is not evidence; measure energy, wrench, tracking, and stop behavior.

Limitations: software and vendor behavior can change after 2026-07-14; geometric simulation does not validate real tracking or contact; time-parameterization documentation does not guarantee torque/jerk compatibility; safety remains subject to the real controller, cell assessment, and qualified review.

Manufacturing Cell Checkpoint

For a tray-to-fixture pick, define a joint home goal, Cartesian pre-grasp, straight guarded approach, grasp action, retreat, and placement. Assign a planning scene owner for table, fixture, tool, and attached object. Record TCP/calibration revision. Use global planning for free-space segments and a separately validated local/contact method only for the final approach.

Log attempt_id, goal_type, frame/stamp, start_state_age, ik_plugin/seed/solution, scene_hash, planner/seed, path_clearance, timing_method, controller, tolerances, update/missed_cycles, speed_scaling, action_result, vendor_fault, final_measured_pose, intervention. KPIs include planning success across seeds, minimum clearance, tracking error, abort rate, cancellation latency, final pose error, contact peak, and recovery time.

What to Learn Next

The complete motion contract is now visible: kinematics creates candidates, planning selects a collision-free path, timing creates a trajectory, controllers execute references, and feedback exposes disagreement. (Chapter 6) explains why Isaac Sim, Isaac Lab, and MuJoCo serve different validation and learning roles, and how to reproduce these contracts across simulators.

Before continuing, take one joint and one Cartesian goal from your cell and fill every table row in this chapter. If a frame, limit, interpolation rule, controller owner, or stop behavior is UNKNOWN, close it in fake hardware or vendor documentation before real motion.

Annotated research trail

These sources deepen planning, control, and contact reasoning. They are grouped by the assumption or experiment to inspect, rather than used as a list of borrowed success rates. Read each result within its platform and protocol.

References

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: A Fast and Robust Implementation of Time-Optimal Path Parameterization for Robots. IEEE Transactions on Robotics. DOI: 10.1109/TRO.2018.2821155.
12. Berscheid, L., & Kröger, T. (2021). Ruckig: Online Trajectory Generation for Jerk-Constrained Robot Motion. Robotics: Science and Systems. DOI: 10.15607/RSS.2021.XVII.015.
13. Chitta, S., et al. (2017). ros_control: A Generic and Simple Control Framework for ROS. 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: An Approach to Manipulation, Part I—Theory. 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. Journal of Dynamic Systems, Measurement, and Control. DOI: 10.1115/1.3139652.
19. Khatib, O. (1987). A Unified Approach for Motion and Force Control of Robot Manipulators. IEEE Journal on Robotics and Automation. DOI: 10.1109/JRA.1987.1087068.
20. Ott, C., et al. (2008). Cartesian Impedance Control of Redundant and Flexible-Joint Robots. IEEE ICRA. DOI: 10.1109/ROBOT.2008.4543616.
21. Casini, D., et al. (2019). Response-Time Analysis of ROS 2 Processing Chains under Reservation-Based Scheduling. ECRTS. DOI: 10.4230/LIPIcs.ECRTS.2019.6.
22. Chen, C., et al. (2025). DexForce: Extracting Force-Informed Actions from Kinesthetic Demonstrations. arXiv:2501.10356.