4. Migrating the Existing Kinematics Model to MoveIt 2#

This notebook explains how the original Dr.QP kinematics model is translated into a MoveIt 2 setup.

The important idea is that the old model is not thrown away. Instead, its assumptions are re-expressed in the formats MoveIt understands:

  • robot geometry becomes URDF/Xacro

  • planning semantics become SRDF

  • inverse kinematics becomes a MoveIt solver configuration

  • trajectory execution is connected to ros2_control

  • launch files assemble the full system

The goal of this notebook is to make that translation easy to follow, especially if you are new to ROS, URDF, or MoveIt.

What existed before MoveIt 2?#

Before MoveIt was added, Dr.QP already had a useful analytical model in drqp_brain.

At a high level:

  • HexapodModel creates six legs with fixed mounting offsets and yaw rotations.

  • each LegModel knows its three link lengths: coxa, femur, and tibia

  • forward_kinematics() computes the foot position from joint angles

  • inverse_kinematics() computes joint angles for a target foot position

That analytical model is excellent for understanding the robot and for building gait logic. MoveIt 2, however, expects a robot description and a planning configuration rather than a handwritten Python solver.

The old mental model in one picture#

The original Python model is based on a simple idea: every leg is the same 3-DOF chain, but each leg is mounted at a different pose on the body.

Note

This is the key observation that makes the migration manageable.

If the six legs share the same internal structure, then the MoveIt 2 model can reuse one leg description many times with different prefixes and mount poses.

In HexapodModel, the six legs are created by changing only two kinds of information:

  • where the leg is attached to the body

  • how the leg is rotated around the body

The actual leg math remains the same for every leg.

Step 1. Preserve the robot’s structure in URDF/Xacro#

MoveIt does not read Python classes like HexapodModel. It reads a robot description, usually generated from URDF and Xacro.

That means the first migration step is to turn the same physical assumptions into links and joints.

1.1 The body file instantiates six legs#

The top-level robot description lives in drqp.urdf.xacro. It mounts the same leg macro six times with different positions and yaw values.

<xacro:leg robot_name="$(arg robot_name)" prefix="left_front" x="0.11692" y="0.06387" yaw="${pi/4}"/>
<xacro:leg robot_name="$(arg robot_name)" prefix="right_front" x="0.11692" y="-0.06387" yaw="${-pi/4}"/>

<xacro:leg robot_name="$(arg robot_name)" prefix="left_middle" x="0" y="0.103" yaw="${pi/2}"/>
<xacro:leg robot_name="$(arg robot_name)" prefix="right_middle" x="0" y="-0.103" yaw="${-pi/2}"/>

<xacro:leg robot_name="$(arg robot_name)" prefix="left_back" x="-0.11692" y="0.06387" yaw="${3*pi/4}"/>
<xacro:leg robot_name="$(arg robot_name)" prefix="right_back" x="-0.11692" y="-0.06387" yaw="${-3*pi/4}"/>

This is the URDF/Xacro equivalent of the location_on_body and rotation fields used by LegModel in Python.

1.2 The leg macro defines the same three actuated joints#

Inside leg.urdf.xacro, each leg contains:

  • a coxa revolute joint around the Z axis

  • a femur revolute joint around the Y axis

  • a tibia revolute joint around the Y axis

Those axes match the old analytical model:

  • alpha -> coxa yaw

  • beta -> femur pitch

  • gamma -> tibia pitch

The URDF also stores joint limits directly on the joints. For example:

<joint name="${robot_name}/${prefix}_coxa" type="revolute">
  <axis xyz="0 0 1"/>
  <limit effort="3" lower="${radians(-90)}" upper="${radians(90)}" velocity="${pi}"/>
</joint>

<joint name="${robot_name}/${prefix}_femur" type="revolute">
  <axis xyz="0 1 0"/>
  <limit effort="3" lower="${radians(-98)}" upper="${radians(90)}" velocity="${pi}"/>
</joint>

<joint name="${robot_name}/${prefix}_tibia" type="revolute">
  <axis xyz="0 1 0"/>
  <limit effort="3" lower="${radians(-80)}" upper="${radians(110)}" velocity="${pi}"/>
</joint>

Important

URDF does not store “how to solve IK”. It only stores the robot’s structure: links, joints, axes, origins, and limits.

1.3 Why there are many fixed joints inside one leg#

If you are new to URDF, the leg file may look more complicated than the Python model because it includes servo bodies, brackets, and several fixed joints.

That is normal.

The analytical model collapses the hardware into three clean segments. The URDF keeps the real mechanical parts because visualization, collision checking, and simulation need the actual physical arrangement.

The important thing is that the three actuated joints are still the same three logical degrees of freedom.

Step 2. Add the semantics MoveIt needs through SRDF#

URDF tells MoveIt what the robot is.

SRDF tells MoveIt how to reason about the robot.

The SRDF for Dr.QP lives in packages/runtime/drqp_moveit/config/drqp.srdf and adds four important layers of meaning.

2.1 Planning groups#

Each leg becomes its own MoveIt planning group, and all six groups are combined into a whole_body group.

Conceptually, this is the MoveIt version of saying:

  • “solve one leg on its own” or

  • “plan for all 18 joints together”

Example:

<group name="left_front_leg">
  <chain base_link="drqp/base_center_link"
         tip_link="drqp/left_front_foot_link"/>
</group>

<group name="whole_body">
  <group name="left_front_leg"/>
  <group name="right_front_leg"/>
  <group name="left_middle_leg"/>
  <group name="right_middle_leg"/>
  <group name="left_back_leg"/>
  <group name="right_back_leg"/>
</group>

2.2 End effectors#

Each foot is declared as an end effector. This gives MoveIt a clean “tip” to target when solving motion for a leg.

2.3 Named states#

The SRDF defines a home state for every leg and for the whole_body group. That gives the planner a known neutral configuration.

2.4 Disabled collision pairs#

MoveIt checks for self-collisions during planning. For a hexapod, some link pairs are always adjacent or can never physically collide.

Those pairs are listed explicitly in the SRDF so MoveIt does not waste time checking impossible collisions.

Tip

This part is easy to underestimate.

If your groups are correct but your self-collision matrix is poor, planning can look broken even though the kinematics are fine.

Step 3. Replace the handwritten IK entry point with a MoveIt solver plugin#

In the analytical model, IK is solved by Python code inside LegModel.inverse_kinematics().

In MoveIt 2, IK is selected per planning group in kinematics.yaml.

For Dr.QP, each leg is a standard serial chain, so the migration uses MoveIt’s KDL plugin instead of writing a custom solver plugin.

# Kinematics solver configuration for Dr.QP hexapod legs.
#
# Each leg is a 3-DOF serial chain (coxa→femur→tibia→foot).
# KDLKinematicsPlugin handles such chains out of the box.
#
# search_resolution  – step size (rad) used when searching for IK solutions
# timeout            – maximum time (s) per IK call
# attempts           – number of random restarts before declaring failure

left_front_leg:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  position_only_ik: true
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

right_front_leg:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  position_only_ik: true
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

left_middle_leg:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  position_only_ik: true
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

right_middle_leg:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  position_only_ik: true
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

left_back_leg:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  position_only_ik: true
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

right_back_leg:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  position_only_ik: true
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

This is one of the most important migration choices:

  • the old system encoded the solver algorithm in Python

  • the new system encodes the solver choice in configuration

That is possible because the URDF/SRDF now describe the kinematic chain well enough for a generic plugin to solve it.

Step 4. Carry over limits and execution constraints#

The old analytical solver knew what kinds of poses were possible because of geometry and angle calculations.

MoveIt still needs explicit planning constraints, especially for trajectory generation and execution.

4.1 Joint limits#

The per-joint planning limits live in joint_limits.yaml. These values work alongside the limits already declared in URDF.

The file covers all 18 joints so MoveIt can apply velocity constraints consistently during planning.

4.2 Controller mapping#

MoveIt plans in terms of joint trajectories, but it still needs to know which ROS action server will execute those trajectories.

That bridge is defined in moveit_controllers.yaml.

# MoveIt 2 controller configuration for Dr.QP.
#
# Connects MoveIt's trajectory execution to the existing
# joint_trajectory_controller managed by ros2_control.
#
# The single JointTrajectoryController covers all 18 joints, so MoveIt
# routes every group's trajectory through the same action server.

moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager

moveit_simple_controller_manager:
  controller_names:
    - joint_trajectory_controller

  joint_trajectory_controller:
    type: FollowJointTrajectory
    action_ns: follow_joint_trajectory
    default: true
    joints:
      - drqp/left_front_coxa
      - drqp/left_front_femur
      - drqp/left_front_tibia
      - drqp/right_front_coxa
      - drqp/right_front_femur
      - drqp/right_front_tibia
      - drqp/left_middle_coxa
      - drqp/left_middle_femur
      - drqp/left_middle_tibia
      - drqp/right_middle_coxa
      - drqp/right_middle_femur
      - drqp/right_middle_tibia
      - drqp/left_back_coxa
      - drqp/left_back_femur
      - drqp/left_back_tibia
      - drqp/right_back_coxa
      - drqp/right_back_femur
      - drqp/right_back_tibia

This is an important architectural point:

  • MoveIt is not replacing ros2_control

  • MoveIt is producing trajectories for the controller that already exists

So the migration is not just “make IK work in RViz”. It is also “connect the planner to the robot’s current execution path”.

Step 5. Assemble everything in the launch files#

Once the URDF, SRDF, solver config, limits, and controller mapping exist, the system still has to be launched with the correct parameters.

That is the job of the launch files in drqp_moveit/launch/.

The key file for understanding the integration is demo.launch.py.

Its job is to load:

  • robot_description from Xacro

  • robot_description_semantic from SRDF

  • robot_description_kinematics from kinematics.yaml

  • robot_description_planning from joint_limits.yaml

  • OMPL planning settings

  • controller configuration

Then it starts:

  • robot_state_publisher

  • joint_state_publisher or joint_state_publisher_gui

  • move_group

  • rviz2

In other words, the launch file is the place where the migration becomes a running system rather than a collection of files.

Step 6. Support both visualization-only and simulation flows#

Dr.QP keeps two beginner-friendly entry points.

6.1 RViz-only demo#

Use this when you want to inspect the robot, move joints manually, and experiment with planning without hardware or Gazebo.

ros2 launch drqp_moveit demo.launch.py

6.2 Gazebo demo#

Use this when you want MoveIt connected to the simulated robot stack.

ros2 launch drqp_moveit demo_gazebo.launch.py

The Gazebo launch reuses the same MoveIt parameters, but runs with simulated time and includes the simulation stack.

This is a strong sign that the migration is properly layered:

  • one robot description

  • one MoveIt configuration package

  • multiple runtime entry points

Step 7. Verify the migration with automated tests#

The migration is also protected by tests in test_moveit_config_package.py.

Those tests check that:

  • the package manifest declares the required dependencies

  • the YAML files parse and expose the expected groups

  • the SRDF declares all six legs plus whole_body

  • the launch files create the expected MoveIt and RViz nodes

  • the runtime parameters include robot description, semantics, kinematics, and planning limits

This matters because MoveIt migrations often fail in configuration, not in code.

For example, a typo in a group name can silently break planning even though the URDF is valid XML. Tests catch that kind of failure early.

Old world vs new world#

The table below is the shortest way to remember the migration.

Old analytical concept

MoveIt 2 representation

HexapodModel leg placement

leg macro instances in drqp.urdf.xacro

LegModel 3-DOF chain

revolute joints in leg.urdf.xacro

handwritten inverse_kinematics()

KDL plugin in kinematics.yaml

foot target for a leg

SRDF chain group with foot tip link

neutral/default pose

SRDF group_state named home

direct angle output

planned trajectory sent through joint_trajectory_controller

ad hoc runtime assembly

launch files that populate move_group parameters

What did not change#

It is easy to think of this migration as “replacing the old kinematics with MoveIt”. That is only partly true.

These things remain the same:

  • the robot still has six identical 3-DOF legs

  • the important joint axes are still one yaw joint and two pitch joints per leg

  • the same joint limits still define what is physically legal

  • higher-level behavior such as gait generation still depends on the same robot structure

What changed is the interface around the model:

  • the robot is now described declaratively

  • IK is selected through plugin configuration

  • planning and collision checking are handled by MoveIt

  • execution is routed through a standard ROS control path

A practical migration checklist#

If you want to repeat this process for another robot, use this checklist:

  1. identify the true actuated joints and their axes

  2. encode the physical structure in URDF or Xacro

  3. define planning groups and end effectors in SRDF

  4. pick a solver plugin for each planning group

  5. declare planning limits and controller mappings

  6. launch move_group with all required robot description parameters

  7. add tests for group names, parameter loading, and launch wiring

Summary#

The Dr.QP migration to MoveIt 2 is best understood as a translation exercise.

The original Python kinematics model supplied the robot knowledge:

  • how legs are mounted

  • how joints move

  • where the feet are

  • which poses are reachable

The MoveIt 2 system takes the same knowledge and spreads it across the files and runtime components that ROS tooling expects.

Once that translation is complete, Dr.QP gains standard planning, collision checking, RViz integration, and controller execution without losing the structure that the earlier notebooks developed step by step.