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 <limit lower="..." upper="..." velocity="..." effort="..."/> 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

<link name="base_link"/>
<joint name="s_joint" type="revolute">
  <parent link="base_link"/>
  <child  link="link_s"/>
  <limit lower="-3.14" upper="3.14" velocity="2.18" effort="0"/>
</joint>
<!-- ... l_joint, u_joint, r_joint, b_joint, t_joint ... -->
<link name="tool0"/>   <!-- chain_tip -->

arm_planning_config.json Reference

Controls the IK solver, OMPL path planner, time parameterizer, and collision manager for each server instance.

Full annotated example:

{
  "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

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

Field	Type	Description
debug	bool	Enable verbose debug logging. Default false.

kinematics_solver.trac_ik

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

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)

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

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

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

Field	Type	Description
max_path_deviation	number (> 0)	Maximum allowed path deviation in degrees.

parameterizer.iptp

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

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

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

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.

{
  "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
}

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.

{
  "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]
  }
}

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.

{
  "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]
  }
}

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.