Robot Setup
===========
URDF Requirements
-----------------
The motion planning node reads the robot URDF from a ``WorkcellDescription`` ROS 2 service at startup. The URDF must satisfy these constraints:
- Contains a valid KDL kinematic chain from ``kinematics.chain_root`` to ``kinematics.chain_tip`` (both set in ``arm_planning_config.json``).
- All joints in the chain have ```` tags.
- Collision geometries are present for all links that participate in self-collision checking (FCL mesh or primitive).
- Joint names are consistent across the URDF, the joint limit service response, and all client requests.
**Typical 6-DOF industrial robot layout**
.. code-block:: xml
arm_planning_config.json Reference
------------------------------------
Controls the IK solver, OMPL path planner, time parameterizer, and collision manager for each server instance.
Full annotated example:
.. code-block:: json
{
"kinematics": {
"chain_root": "base_link",
"chain_tip": "tool0"
},
"misc": {
"debug": false
},
"kinematics_solver": {
"trac_ik": {
"timeout": 0.02,
"epsilon": 1e-5
}
},
"path_planner": {
"ompl": {
"is_planning_offline": false,
"online_planning": {
"planner_name": "RRTConnect",
"longest_valid_segment": 0.01,
"max_time_allowed": 0.8,
"hybridize": false,
"max_num_threads": 6
},
"offline_planning": {
"planner_name": "RRTstar",
"longest_valid_segment": 0.001,
"max_time_allowed": 2.0,
"hybridize": true,
"max_num_threads": 6
},
"RRTConnect": {
"range": 0.0
}
}
},
"parameterizer": {
"toppra": {
"algorithm_discretization_num": 200,
"max_tries": 5,
"max_path_deviation": 5
},
"totg": {
"max_path_deviation": 5
},
"iptp": {
"max_iterations": 1000,
"max_time_change_per_iteration": 0.001
}
},
"collision_manager": {
"num_max_contacts": 1,
"enable_contact": false
},
"trajectory_filter": {
"smoothing_coef": [0.25, 0.5, 1.0, 0.5, 0.25]
},
"cartesian_planner": {
"use_diagonal": false
}
}
Field reference
~~~~~~~~~~~~~~~
**kinematics**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``chain_root``
- string
- URDF link name for the root of the KDL chain (e.g. ``"base_link"``). Required when running the full node.
* - ``chain_tip``
- string
- URDF link name for the tip of the KDL chain (e.g. ``"tool0"``). Required when running the full node.
**misc**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``debug``
- bool
- Enable verbose debug logging. Default ``false``.
**kinematics_solver.trac_ik**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``timeout``
- number (> 0)
- IK solver timeout in seconds. Typical value: ``0.02``. Used when OPW params are not provided or a solution is out of OPW range.
* - ``epsilon``
- number (> 0)
- Cartesian precision tolerance for the IK solver. Typical value: ``1e-5``.
**path_planner.ompl**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``is_planning_offline``
- bool
- ``true`` → use ``offline_planning`` config (RRTstar). ``false`` → use ``online_planning`` config (RRTConnect).
* - ``online_planning.planner_name``
- enum
- OMPL planner algorithm. One of: ``"RRTConnect"`` (default), ``"RRT"``, ``"RRTstar"``, ``"PRM"``, ``"PRMstar"``, ``"LBTRRT"``, ``"BiTRRT"``, ``""``.
* - ``online_planning.longest_valid_segment``
- number (> 0)
- Maximum collision-check step size along the path in radians. Smaller = safer but slower.
* - ``online_planning.max_time_allowed``
- number (> 0)
- Planning timeout in seconds. Typical online value: ``0.2``–``1.0``.
* - ``online_planning.hybridize``
- bool
- Apply post-processing path smoothing after planning.
* - ``online_planning.max_num_threads``
- int (≥ 1)
- Number of parallel OMPL planning threads per server.
* - ``offline_planning.planner_name``
- enum
- One of: ``"RRTstar"``, ``"RRTConnect"``, ``"PRM"``, ``"PRMstar"``, ``""``.
* - ``RRTConnect.range``
- number (≥ 0)
- RRTConnect range parameter. ``0`` = automatic selection by OMPL.
**path_planner.astar** (offline graph-based planner)
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``weight_mag``
- number
- Heuristic weight magnitude.
* - ``pos_dist_mag``
- number
- Position distance weighting in the heuristic.
* - ``ori_dist_mag``
- number
- Orientation distance weighting in the heuristic.
* - ``n_adjs``
- int (≥ 1)
- Graph adjacency levels.
* - ``data_path``
- string
- Absolute path to the offline graph data directory.
* - ``max_num_threads``
- int (≥ 1)
- Parallel search threads.
**path_planner.joint_coupling**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``longest_valid_segment``
- number (> 0)
- Collision-check step size in radians for coupled joint motion.
* - ``max_num_threads``
- int (≥ 1)
- Parallel planning threads.
**parameterizer.toppra**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``algorithm_discretization_num``
- int (≥ 2)
- Number of path discretization points. Higher = more accurate but slower.
* - ``max_tries``
- int (≥ 1)
- Maximum retry attempts if TOPPRA fails.
* - ``max_path_deviation``
- number (> 0)
- Maximum allowed path deviation in degrees.
**parameterizer.totg**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``max_path_deviation``
- number (> 0)
- Maximum allowed path deviation in degrees.
**parameterizer.iptp**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``max_iterations``
- int (≥ 1)
- Maximum IPTP iterations.
* - ``max_time_change_per_iteration``
- number (> 0)
- Maximum time delta allowed per iteration in seconds.
**collision_manager**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``num_max_contacts``
- int (≥ 0)
- Maximum contacts to report per collision check. ``0`` = unlimited.
* - ``enable_contact``
- bool
- Retrieve full contact information (positions, normals). Only needed if you call ``CheckRobotCollision`` with ``get_contact_infos=true``.
**trajectory_filter**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``smoothing_coef``
- number[]
- Convolution kernel applied to joint trajectories after parameterization. Coefficients must sum to approximately 1.0. Example: ``[0.25, 0.5, 1.0, 0.5, 0.25]``.
**cartesian_planner**
.. list-table::
:header-rows: 1
:widths: 30 12 58
* - Field
- Type
- Description
* - ``use_diagonal``
- bool
- Use diagonal Cartesian interpolation mode.
motion_planning_config.json Reference
--------------------------------------
Controls the server pool and ROS 2 service layer. All fields are optional; defaults shown.
.. code-block:: json
{
"collision_servers_num": 2,
"fk_servers_num": 2,
"ik_servers_num": 4,
"motion_servers_num": 4,
"parameterizer_servers_num": 2,
"qos_service_queue_depth": 10,
"workcell_service_timeout_sec": 10
}
.. list-table::
:header-rows: 1
:widths: 38 12 50
* - Field
- Default
- Description
* - ``collision_servers_num``
- 1
- CollisionServer pool size. Each instance handles one concurrent collision-check request.
* - ``fk_servers_num``
- 1
- FKServer pool size.
* - ``ik_servers_num``
- 1
- IKServer pool size.
* - ``motion_servers_num``
- 1
- MotionServer pool size. Increase for parallel motion planning requests.
* - ``parameterizer_servers_num``
- 1
- ParameterizerServer pool size.
* - ``qos_service_queue_depth``
- 10
- ROS 2 service QoS queue depth.
* - ``workcell_service_timeout_sec``
- 10
- Seconds to wait for the ``WorkcellDescription`` service during startup.
OPW Kinematics Parameters Reference
-------------------------------------
OPW provides fast analytic IK for standard 6-DOF industrial robots. These parameters describe the Denavit-Hartenberg geometry of the robot. Values are in metres and radians.
.. code-block:: json
{
"opw_kinematics": {
"a1": 0.075,
"a2": -0.042,
"b": 0.0,
"c1": 0.495,
"c2": 0.640,
"c3": 0.600,
"c4": 0.11,
"offsets": [0, 0, 0, 0, 0, 0],
"sign_corrections": [1, 1,-1, 1, 1, 1]
}
}
.. list-table::
:header-rows: 1
:widths: 22 12 66
* - Field
- Type
- Description
* - ``a1``
- number
- Parallel shift between axes 1 & 2 (metres). Typically small.
* - ``a2``
- number
- Parallel shift between axes 2 & 3 (metres). Typically negative.
* - ``b``
- number
- Wrist offset b (metres). ``0`` for most industrial robots.
* - ``c1``
- number (> 0)
- Shoulder height — distance from base to axis 2 (metres).
* - ``c2``
- number (> 0)
- Upper arm length — distance between axes 2 & 3 (metres).
* - ``c3``
- number (> 0)
- Forearm length — distance between axes 3 & 4 (metres).
* - ``c4``
- number (> 0)
- Wrist length — distance from axis 4 to flange (metres).
* - ``offsets``
- number[6]
- Per-joint angular offsets in radians. Applied on top of joint positions before OPW calculation.
* - ``sign_corrections``
- int[6] (±1)
- Per-joint sign corrections. Aligns OPW joint convention with the URDF joint direction. Each value must be ``1`` or ``-1``.
.. tip::
OPW parameters for common Yaskawa / FANUC / KUKA / ABB robots are published in the ``opw_kinematics`` ROS package. Start there before deriving them from a URDF.
Inverse Dynamics Config Reference
-----------------------------------
Required only when using TOPPRA with torque constraints. Leave the path empty (``""``) in the launch file otherwise.
.. code-block:: json
{
"inverse_dynamics_config_stamped": {
"use": true,
"inverse_dynamics_shared_lib": "/path/to/libmy_id_model.so",
"derived_model_scoped_class_name": "my_ns::MyIDModel",
"base_params": [1.0, 2.0, 3.0]
}
}
.. list-table::
:header-rows: 1
:widths: 38 12 50
* - Field
- Required
- Description
* - ``use``
- Yes
- ``true`` to enable torque-constrained TOPPRA. When ``false``, all other fields are ignored.
* - ``inverse_dynamics_shared_lib``
- If ``use=true``
- Absolute path to the ``.so`` implementing the inverse dynamics model interface.
* - ``derived_model_scoped_class_name``
- If ``use=true``
- Fully-scoped class name to instantiate from the shared library (e.g. ``"my_ns::MyIDModel"``).
* - ``base_params``
- No
- Robot-specific dynamic model parameters passed to the constructor.