Kinematics and Motion Planning

IK solvers and when each is the right one, the planning libraries MoveIt offers, servo for real-time control, and pick and place.
Author

Benedict Thekkel

Forward and Inverse Kinematics

Forward kinematics is the easy direction: joint angles to end-effector pose, a chain of matrix multiplications. It is computed numerically from a URDF in ../03_Spatial_and_Temporal/01_Robot_Description.ipynb, and it is what robot_state_publisher does every cycle.

Inverse kinematics is the hard direction: a pose to joint angles. It is hard because it is generally not a function.

  • Multiple solutions. A 6-DOF arm typically has up to 8 solutions for a reachable pose: elbow up or down, wrist flipped, shoulder forward or back. Which one the solver returns matters, because two solutions that are both correct can be far apart in joint space, and moving between them is a large motion.
  • No solution. Outside the workspace, or blocked by a joint limit.
  • Infinite solutions. A 7-DOF arm has a null space: a continuum of joint configurations for one pose, which is what lets it avoid obstacles while holding the gripper still.
  • Singularities. Configurations where the arm loses a degree of freedom and joint velocities go to infinity for a finite Cartesian velocity. A fully extended elbow is the obvious one.

kinematics.yaml chooses the solver per group:

arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3
Solver Method Use when
KDL numerical, Newton-based the default; works on any chain, slow, can fail on a solvable pose
TRAC-IK numerical, two concurrent solvers plus a SQP fallback a near-drop-in upgrade on KDL; markedly better convergence
IKFast analytic, generated per robot fastest by far, exact, but requires generating a plugin for that exact arm
BioIK / pick_ik optimisation, with goal weights redundant arms, or extra objectives such as staying away from limits
vendor plugin analytic most industrial arms ship one; prefer it

The practical advice: if IK fails on poses you believe are reachable, change the solver before anything else. KDL is a numerical solver with a timeout, so it genuinely fails on solvable poses, and a TRAC-IK or pick_ik swap is two lines of YAML. It is a much better first move than raising the timeout.

The Planning Libraries

MoveIt delegates the search to a planning plugin, and the three in common use solve different problems.

Pipeline Method Good for Watch out for
OMPL sampling-based (RRTConnect, and others) the default; cluttered scenes, high DOF paths are random and non-repeatable, and need smoothing
Pilz Industrial Motion Planner analytic: PTP, LIN, CIRC repeatable, predictable industrial motion no obstacle avoidance; it plans the commanded shape or fails
CHOMP trajectory optimisation smoothing an existing path needs a seed; local minima
STOMP stochastic optimisation similar to CHOMP same 
# ompl_planning.yaml
arm:
  default_planner_config: RRTConnectkConfigDefault
  planner_configs:
    RRTConnectkConfigDefault:
      type: geometric::RRTConnect
      range: 0.0          # 0 = let OMPL choose the step size

The defining property of OMPL is worth stating plainly: it is a sampling planner, so the same request gives a different path each time. That is not a bug, it is the algorithm, and it has consequences that surprise people coming from industrial robotics:

  • Paths are not repeatable, so a motion that cleared an obstacle yesterday may graze it today. If repeatability is a requirement, Pilz is the answer, not OMPL with a fixed seed.
  • Paths wander. Post-processing (CHOMP, STOMP, or MoveIt’s own shortcutter) is normal, not optional.
  • allowed_planning_time is a real dial. A sampling planner fails by running out of time, so a hard scene may just need 5 seconds rather than the default 1.
  • Cartesian paths are a separate API. computeCartesianPath interpolates in Cartesian space with IK at each step, which is what you want for a straight-line insertion; it returns the fraction it achieved, and a fraction below 1.0 means an incomplete path that must not be executed blindly.

Servo

moveit_servo is the real-time path: it takes a stream of Cartesian or joint velocity commands and produces joint commands at a fixed rate, with collision checking and singularity handling, without planning at all.

moveit_servo:
  ros__parameters:
    move_group_name: arm
    command_in_type: "speed_units"       # or "unitless"
    publish_period: 0.01                 # 100 Hz
    command_out_type: trajectory_msgs/JointTrajectory
    scale:
      linear: 0.4
      rotational: 0.8
    lower_singularity_threshold: 17.0
    hard_stop_singularity_threshold: 30.0
    collision_check_rate: 10.0
    self_collision_proximity_threshold: 0.01

Use it for teleoperation, visual servoing, force-guided insertion, or anything where the target moves continuously. Do not use it to reach a distant goal: it has no planner, so it will drive the arm straight at the target and stop at the first obstacle or singularity.

The singularity thresholds are the safety-relevant parameters. Approaching a singularity, servo scales the commanded velocity down (lower_singularity_threshold) and then halts (hard_stop_singularity_threshold). Raising them to make the arm “more responsive” near full extension is how a joint gets commanded at its velocity limit.

Trajectory Execution

MoveIt’s output is a trajectory_msgs/JointTrajectory, sent as a FollowJointTrajectory action to a joint_trajectory_controller from ros2_control. That is the whole interface between planning and hardware, and it is why an arm brought up with joint_trajectory_controller works with MoveIt without further plumbing; see ../04_Simulation_and_Hardware/02_ros2_control.ipynb.

ros2 action list | grep follow_joint_trajectory
ros2 topic echo /arm_controller/controller_state --once

Three failure modes at this boundary:

  • Tolerance violations. joint_trajectory_controller aborts if the arm falls outside path_tolerance or goal_time_tolerance. On real hardware with friction and gravity sag, default tolerances are often too tight, and the fix is either better tuning or honest tolerances, not disabling the check.
  • Trajectories too aggressive to track. MoveIt parameterised against joint_limits.yaml, so if those exceed what the hardware achieves, every execution aborts partway.
  • Start-state mismatch. A trajectory whose first point is not where the arm actually is gets rejected, which happens when /joint_states is stale or when a plan is executed long after being computed.

Pick and Place

MoveIt 2 dropped ROS 1’s monolithic pick() and place() in favour of explicit task construction. Pick and place is a sequence, and writing it out is clearer than a single call with thirty parameters:

approach -> pre-grasp pose -> open gripper -> move to grasp (Cartesian, slow)
  -> close gripper -> ATTACH object to the gripper in the planning scene
  -> retreat (Cartesian) -> transit plan to the place location
  -> approach place -> open gripper -> DETACH object -> retreat

MoveIt Task Constructor (moveit_task_constructor) is the framework for exactly this: stages composed into a pipeline, where each stage generates or filters solutions, and the whole task is planned before anything executes. It is worth the learning curve as soon as the sequence has more than one branch, because it plans the whole task rather than discovering at the retreat that the grasp pose made the rest impossible.

Things that matter more than the planner here:

  • Attach and detach at the right moments. The planning scene must know the object is held, or the arm swings it into the table; see 04_MoveIt2_Overview.ipynb.
  • Allow contact with the object being grasped. The gripper has to touch it, so collision between the fingers and the target must be permitted for that phase (ALLOWED_COLLISION_MATRIX), or no grasp pose is ever valid.
  • Approach and retreat are Cartesian and slow. A sampling planner near the object will take a curved path into it; use computeCartesianPath for the last few centimetres, and check the returned fraction.
  • Grasp pose generation is the real problem. Where to grab an object is perception and geometry, not motion planning. moveit_grasps offers heuristics for simple shapes; anything else is a perception task, covered in ../05_Perception/02_Inference_Node_Integration.ipynb.
  • Gripper control is usually separate. Most grippers are a GripperCommand action or a plain service, not a joint in the arm group.
Back to top