Empirical Evaluation of CHOMP for Autonomous Pick-and-Place Manipulation Using a UR5e Robot Arm: A MATLAB–ROS2 Hybrid Framework

Abstract

This study investigated the application of Covariant Hamiltonian Optimization for Motion Planning (CHOMP) in the MATLAB programming environment to an industrially relevant Universal Robots UR5e six-DOF manipulator. The parameters were set to be equal to those of the standard MoveIt2 CHOMP plugin, and the obstacle cost was computed directly in MATLAB using the Robotics System Toolbox’s forward kinematics function to obtain the end-effector position at each trajectory waypoint, which was then evaluated against a piecewise potential field defined over three spherical obstacles in the workspace. We executed five distinct picking task examples and one task over thirty trials, together with a sensitivity analysis over the weight parameter defining the optimization smoothness (i.e., weight/gamma). The mixed empirical results exposed major drawbacks of vanilla CHOMP under our parameter configuration. We achieved a collision-free result for only two of the five tasks, T-03 and T-05, with T-03 converging quickly in five iterations (0.16 s) and T-05 requiring 156 iterations and hitting the planning timeout limit of 10 s. Three tasks did not yield any collision-free result under the 10 s time limit. One of those three tasks, T-01, when running 30 random trial simulations after adding a tiny amount of noise to the start/end poses, yielded 0%, so all trials timed out on its planning 200-iteration limit with an invalid collision result. We analyzed the movement profile (position, velocity, and acceleration over time) of the trajectories generated during the experiments. Several examples exceed the UR5e velocity limit (180 deg/s) and the UR5e acceleration limit (400 deg/s²) by an order of magnitude, and peak values on T-02 reached up to 4731.92 deg/s². With these chosen parameters, basic CHOMP is not industrially suitable for the UR5e robot or for the implementation of the empirical evaluation of CHOMP discussed in this paper. We also identified the modes of failure of basic CHOMP under these parameters and discuss relevant changes.

1. Introduction

Generating a trajectory that a physical manipulator can execute is one of the biggest open problems in industrial robotics. A planner that looks good on paper or works on benchmark problems can fail catastrophically once it is tried on a real manipulator with constraints that are more nuanced—for example, in terms of its joint angles, limited workspace, or a wide variety of intended applications. Over the past decades, the motion planning community has invested considerable effort into optimization-based planners, with Covariant Hamiltonian Optimization for Motion Planning (CHOMP) (Ratliff et al., 2009 [1]; Zucker et al., 2013 [2]) and its stochastic cousin Stochastic Trajectory Optimization for Motion Planning (STOMP) (Kalakrishnan et al., 2011 [3]) being among the most widely cited. These methods are attractive because they directly optimize a cost functional that explicitly penalizes non-smooth motion and proximity to obstacles, producing trajectories that are, in principle, smoother and more predictable than those generated by sampling-based planners. The sampling-based family originates with the Rapidly exploring Random Tree (RRT) [4] and its optimal variant RRT* [5], with RRT-Connect [6] and the improved RRT*-Connect [7] among the most widely used for manipulator planning. The optimization-based family has since been extended in several directions, notably by formulating planning as probabilistic inference over continuous-time trajectories represented by sparse Gaussian processes [8], which relaxes the fixed discretization density that a waypoint-based formulation such as CHOMP must commit to a priori [9]. A parallel line of work replaces explicit cost-functional design with learned policies; progress in that direction is reviewed by Elguea-Aguinaco et al. [10] and Urbaniak et al. [11].

However, the fact that an algorithm has been published and integrated into a popular stack like MoveIt2 does not imply that its default hyperparameters will produce usable trajectories for any given robot or task. The present work was motivated by that observation. The long-term goal of this research is the development of an adaptive trajectory optimizer that can be benchmarked fairly against CHOMP, open motion planning (OMPL) [12], RRT-Connect, and the Pilz Industrial Motion Planner [13] within the MoveIt2 ecosystem.

However, an adaptive implementation of CHOMP has not yet been evaluated on the target hardware. We aim to accurately characterize the behavior of CHOMP on the UR5e under MoveIt2 parameters, reporting its shortcomings under fair conditions so that an adaptive version may be built upon sound empirical evidence.

We limit CHOMP’s use to an implementation entirely contained within MATLAB to avoid implementation details unique to the Robot Operating System 2 (ROS2) ecosystem influencing the results. We took parameters directly from the MoveIt2 YAML configuration for CHOMP, computed obstacle costs through the forward kinematics (fk) position of the UR5e end effector (tool0) in the UR5e utilizing the MATLAB Robotics System Toolbox, and ran each planner up to 200 iterations or until it hit a wall-clock limit of 10 s per task. We included five tasks that are representative of common pick-and-place operations. The first of these was subjected to a 30-trial statistical analysis, introducing small random disturbances in both the start and goal configurations to investigate CHOMP’s sensitivity to noise.

The experimental results showed that vanilla CHOMP performed poorly on this task. CHOMP cleanly completed and converged quickly on only one task. Two others reached the convergence criterion but only after hitting the time limit. Two others failed completely before the time limit. In our robustness experiment, we observed a 0% success rate over 30 trials. Multiple tasks displayed peak joint velocities far exceeding the UR5e manufacturer limits, as did several observed peak accelerations. Most observed failures resulted in CHOMP finding a path that was not remotely usable on real hardware, without even any attempt to approximate these constraints. The primary aim of this paper is to try to understand why these issues occur and to propose a path toward remedying them. The present study addresses this gap directly. Rather than proposing a new optimizer and asserting its superiority over an under-examined baseline, we first subject baseline CHOMP—configured identically to the MoveIt2 defaults—to a deliberate quantitative failure analysis on a UR5e manipulator.

This study consists of three parts: (1) We present an honest empirical study of CHOMP under MoveIt2’s defaults on a UR5e model. We carefully document the number of failed optimization runs and the nature of their failures. In particular, we document kinematic limit violations that are relevant for real-world applications. (2) We analyze the underlying reasons for these failures and relate them to known theoretical drawbacks of the CHOMP formulation, especially concerning the reliance on the learning rate and the lack of explicit kinetic limit incorporation [14]. (3) Based on these failures, we propose algorithmic ideas for a future adaptive CHOMP variant: an adaptive learning rate based on progress, kinematic penalty regions, and a velocity-aware smoothness term. These directions are outlined in Section 4 as the basis for follow-up work motivated directly by the failure modes characterized here. This paper is, therefore, the diagnostic first stage of an adaptive-CHOMP program: its purpose is to establish, on sound empirical footing, what an improved planner must fix and why.

The rest of this paper is organized as follows. The theory behind CHOMP and the performance measures used are described in Section 2; the MATLAB-ROS2 system setup and details about the experiments, the environment, and the chosen tasks are described in Section 3; our empirical results are described in Section 4, focusing on analyzing failure modes and providing a sensitivity analysis; and discussion of these results and our suggestions for an adaptive CHOMP, while acknowledging limitations of the current study, are given in Section 5 along with conclusions.

2. Materials and Methods

Figure 1 outlines the experimental pipeline. Ubuntu 22.04 LTS on an HP Pavilion i7 is the base operating environment. Setup phase (blue): MATLAB R2025b is now named explicitly as the main engine, with the two Toolbox [15] calls shown as separate badges—loadrobot for pulling the UR5e URDF and getTransform for the FK queries. spdiags is identified as the specific MATLAB function used to build the sparse smoothness K. Optimizer loop (red): the FK numerical Jacobian step is labeled inside the gradient box to make it clear that the Robotics System Toolbox is being called on every waypoint of every iteration. ROS2 branch (amber): The optional benchmarking diamond now explicitly branches—the yes-path includes ROS2 Humble [16], MoveIt2 [17], Gazebo Classic 11, and its three planner plugins (OMPL [12], CHOMP [1,2], and Pilz [13]), which would be timed if the stack were running. The no-path bypasses this and leads to the output; this is what happened in the actual reported run.

Figure 1. Experimental pipeline.

2.1. Hardware and Software

All experiments were run on an HP Pavilion Gaming Laptop (Intel Core i7, 16 GB RAM, and NVIDIA GTX 1650) under Ubuntu 22.04 LTS. MATLAB R2025b was used with the Robotics System Toolbox and Optimization Toolbox. The intended ROS2 stack is ROS2 Humble Hawksbill [16], with MoveIt2 version 2.5.9 and Gazebo Classic, using the Ur_description and Ur_gazebo packages with a Robotiq 2F-85 gripper (Robotiq, Lévis, QC, Canada) attached at the tool0 flange. The present paper reports MATLAB-only results; the ROS2 benchmarking comparison was not available at the time of this experimental run because the ROS2 stack was not active during the measurement session. This is an action for future work.

The UR5e [15] (Figure 2) is a widely adopted six-DOF collaborative arm with an open, well-documented URDF and strong support in MoveIt2, ROS2, and MATLAB’s Robotics System Toolbox [18], making it easy to integrate across a hybrid pipeline. It is also a common benchmark platform in the manipulation planning literature [19,20].

Figure 2. UR5e robot dimensions. All dimensions are in mm. For public use. (Created by Universal Robots [15]).

2.2. MATLAB Implementation

The optimizer is implemented in a single MATLAB script of approximately 700 lines organized into ten sections: parameter loading; UR5e model construction via loadrobot; task definition; obstacle placement; finite-difference kernel assembly; the main CHOMP loop; a 30-trial statistical study; a smoothness-weight sensitivity sweep; optional ROS2 benchmarking; and tabulation and figure generation. The Robotics System Toolbox [18] is used to load the UR5e URDF and to compute forward kinematics via the getTransform function with DataFormat set to “row”. The experimental platform follows a MATLAB–ROS2 hybrid approach, in which MATLAB serves as the optimization engine communicating with the ROS2 planning stack [21].

2.3. End-Effector Link

The provided code represents an attempt to deduce the “end-effector link name”, starting with tool0 and falling back to wrist_3_link if no such tool0 link exists. In the execution run that led to the reported issue, tool0 was detected and successfully utilized. This detail may be significant, since prior iterations of this project had incorrectly specified “tool0_controller” (a non-existent link in the default URDF), resulting in a failed FK, a silent error, and a makeshift joint-space distance metric that proved to be utterly nonsensical.

2.4. Theoretical Formulation

The baseline CHOMP implementation: trajectories are 50-waypoint joint-space sequences optimized via a composite cost (smoothness + FK-based obstacle potential). The performance is then tracked through convergence iterations, planning time, path length, and peak joint velocity/acceleration—the same metrics that later expose CHOMP’s kinematic limit violations, motivating the adaptive variant.

2.4.1. Trajectory Representation

A trajectory is represented as a sequence of N = 50 configuration vectors in the joint space of a six-DOF manipulator, where q_i denotes the i-th configuration of the joint angles, and the endpoints, q_0 = q_start and q_{N − 1} = q_goal, are fixed boundary conditions corresponding to the manipulator’s initial and target poses.

xi = {q_0, q_1, ..., q_{N − 1}}, q_i ∈ R^6

(1)

The endpoints q_0 = q_start and q_{N − 1} = q_goal are fixed boundary conditions. The waypoints are sampled uniformly with index t ∈ [0,1] and dt = 1/(N − 1).

2.4.2. Composite Cost Functional

CHOMP [1,2] minimizes a weighted sum of smoothness and obstacle costs:

C(xi) = w_s · F_smooth(xi) + w_o · C_obs(xi)

(2)

where w_s = 0.1 and w_o = 1.0, as configured in the MoveIt2 YAML.

2.4.3. Smoothness Kernel

The smoothing cost penalizes the second power of acceleration along the path with the use of a second-order differentiation matrix A with dimensions (N − 2) × N. The smoothness kernel is K = A^T A ∈ R^{N × N}, and the smoothness cost and gradient are

F_smooth(xi) = ½ · trace(xi^T K xi), ∇ F_smooth = K xi

(3)

2.4.4. Forward Kinematics (FK)-Based Obstacle Cost

Forward kinematics was used to calculate the end-effector position at the i-th waypoint q_i through the use of the MATLAB Robotics System Toolbox function getTransform on the loaded UR5e unified robot description format (URDF). The distance(d) from the end effector to each spherical obstacle is evaluated, and a piecewise potential field is applied:

c(d) = d_clear − d_thresh,        d < d_thresh       
c(d) = (d_clear − d)²/(2 (d_clear − d_thresh)), d_thresh ≤ d < d_clear
c(d) = 0,                 d ≥ d_clear     

(4)

where d_clear = 0.2 m and d_thresh = 0.07 m. The gradient with respect to joint angles is computed by numerical Jacobian finite differencing using a step size of 10⁻⁴ rad.

2.4.5. Update Rule and Constraints

The resulting total gradient leads to a vanilla gradient descent update with joint-wise clamping of the maximal step size to 0.1 radians and with optional addition of Gaussian noise:

xi ← xi − η · clamp(∇C + ε)

(5)

where the learning rate η = 0.01. Joint limits are enforced by element-wise clipping after each update, and the boundary conditions are re-applied to xi(1,:) and xi(N,:). Convergence is declared when the obstacle cost remains below 10⁻⁴ for five consecutive iterations.

2.4.6. Time Normalization

The smoothness cost F_smooth (Equation (3)) and the convergence criterion are computed on the trajectory’s normalized index representation—the sum of squared second differences between adjacent waypoints. A small F_smooth therefore indicates only that consecutive waypoints are close together in joint-angle space; it places no bound on the physical velocity or acceleration that would result from executing the trajectory in real time. To obtain the physical values reported in Table 3, each 50-waypoint trajectory was mapped onto a nominal one-second execution window (Δt = 1/49 s per waypoint), with joint velocity and acceleration estimated by central finite differences, v_i ≈ (q_{i + 1} − q_{I − 1})/(2Δt) and a_i ≈ (q_{i + 1} − 2q_i + q_{i − 1})/Δt². Because this conversion divides small index-space differences by Δt² ≈ 4.2 × 10⁻⁴, a near-zero smoothness cost is fully compatible with peak accelerations of several thousand deg/s². This separation between joint-space geometry and executable dynamics is not specific to CHOMP, as outlined in Dobiš et al. [22].

3. Results

3.1. Experimental Setup

The experimental setup—consisting of five pick-and-place task configurations of increasing complexity, a cluttered workspace containing three obstacles, and fixed MoveIt2 default planner parameters—provides a controlled, reproducible environment for the comparison of baseline and adaptive CHOMP and the evaluation of the violations of kinematic and convergence behavior (reported in subsequent sections of the paper). Moreover, task T-01 is used for statistical studies consisting of 30 trials with start and/or goal poses that are slightly perturbed from the set task pose, thus allowing for the evaluation of the robustness of baseline vs. adaptive CHOMP.

3.1.1. Task Configurations

Five pick-and-place configurations were selected according to three criteria: (i) coverage of a range of joint-space displacement magnitudes, from short repositioning motions to full-workspace transfers; (ii) inclusion of a controlled pair sharing an identical initial configuration, isolating the effect of goal pose on optimizer behavior; and (iii) inclusion of a reversed trajectory, to assess whether CHOMP convergence is symmetric under inversion of the start and goal.

The included task configurations are labeled as tasks T-01, T-02, T-03, T-04, and T-05. These tasks cover varying degrees of complexity in their motion trajectories: tasks T-01 and T-03 begin from the identical “home” pose but end at different target poses; task T-02 continues the trajectory from T-01’s target pose to return to the initial home pose; task T-04 simulates a longer transfer; and task T-05 executes a transfer along the diagonal across the work area, as shown in Table 1.

Table 1. Five pick-and-place task configurations evaluated in this study.

ID	Task Name	Description
T-01	Home to Pick-A	From home pose to first grasp location
T-02	Pick-A to Place-A	From first grasp to first drop location
T-03	Home to Pick-B (Short)	Short-range motion to a nearby grasp
T-04	Pick-B to Place-B (Long)	Long-range transfer across workspace
T-05	Diagonal Workspace	Diagonal traversal spanning multiple regions

3.1.2. Obstacle Configuration

Obstacles were placed at locations approximating a cluttered workstation environment: Obstacle 1 is centered at (0.30, 0.20, 0.40) m with a radius of 0.08 m; Obstacle 2 is centered at (0.10, −0.30, 0.30) m with a radius of 0.10 m; and Obstacle 3 is centered at (0.40, 0.00, 0.20) m with a radius of 0.06 m. These positions were chosen to force non-trivial avoidance behavior on the part of the optimization algorithm, rather than to replicate specific clutter.

3.1.3. Optimizer Parameters

CHOMP hyperparameters were configured identically to the MoveIt2 defaults: smoothness weight was set at 0.1, obstacle weight at 1.0, learning rate at 0.01, joint update limit at 0.1 radians, clearance distance was defined as 0.2 m, collision threshold maintained at 0.07 m, a maximum of 200 iterations, and stochastic descent enabled with a random jump amount of 1.0. Planning was terminated at either the 200-iteration limit or a 10 s wall-clock limit, whichever came first. Quintic spline initialization was used throughout.

3.1.4. Statistical Study

For task T-01, an additional 30-trial statistical analysis was run. In each trial, the start and goal configurations were perturbed by zero-mean Gaussian noise with a standard deviation of 0.03 rad on each joint, clipped to the joint limits. The random seed was fixed using (rng(42)) (commonly in MATLAB or other languages). Planning time, iteration count, final cost, path length, and collision-free status were recorded for each trial.

3.2. Experimental Results

The per-task convergence and kinematic results show that baseline CHOMP achieved collision-free planning in only 2 of 5 tasks and violated UR5e velocity or acceleration limits in 4 of 5—exposing the gap between optimizer-reported success and physical executability.

3.2.1. Per-Task Convergence Behavior

Table 2 summarizes the outcomes of the five per-task runs. The results should be read carefully: they do not show a uniformly successful planner. Of the five tasks, only T-03 converged quickly to a collision-free trajectory within the expected iteration budget. T-05 produced a collision-free final trajectory but required 156 iterations and reached the 10 s wall-clock limit before the sustained collision-free convergence criterion was satisfied, which means it is labeled as converged by the output statistics but came extremely close to failing. Tasks T-01, T-02, and T-04 all hit the 10 s time limit and terminated with their obstacle cost still above the 10⁻⁴ threshold, i.e., they failed to produce a collision-free trajectory within the configured compute budget.

Table 2. MATLAB output—CHOMP optimization results across five tasks. Three of five tasks failed to achieve collision-free trajectories within the 10 s time budget.

Task	Iters	Time (s)	Final Cost	Path (Rad)	Collision-Free
T-01	115	10.0273	0.022884	1.7098	No (timeout)
T-02	137	10.0265	0.018405	2.5744	No (timeout)
T-03	5	0.1634	0.000004	0.7050	Yes
T-04	154	10.0748	0.001034	2.7793	No (timeout)
T-05	156	10.0707	0.000184	2.3313	Yes (at limit)

A useful way to read Table 2 is to compare T-01 and T-03. Both begin at the same “home” pose but target different goals. T-03 lies in a short-range region of the configuration space where the quintic spline initialization already clears all three obstacles, so CHOMP converges almost immediately, requiring only five iterations over 0.16 s, with a final cost of 4 × 10⁻⁶. Target T-01 aims to bring the quintic close to the obstacle at (0.30, 0.20, 0.40) with insufficient steps. T-02 and T-04 exhibit similar behavior, where large joint movements push through congested areas with obstacles near the end-effector work trajectory.

3.2.2. Peak Kinematic Values

The computed peak joint velocities and accelerations for each task are presented in Table 3. These calculations were derived using central finite differences from the generated optimal path, thus representing the instructions the controller would convey to the robot. For reference, UR5e robots are characterized by a peak joint velocity of 180°/s (3.14 rad/s) and a typical peak joint acceleration of 400°/s² [23]. The maximum values observed during these experiments and their correspondence to these performance specifications are included in the final column.

Table 3. Peak kinematic values versus UR5e manufacturer limits (180 deg/s, 400 deg/s²). Four of five tasks exceed acceleration limits by a factor of at least 4.3×. Computed under a nominal 1 s execution window.

Task	Peak Vel (deg/s)	Peak Accel (deg/s2)	Limit Check
T-01	183.73 (J1)	1980.63 (J3)	Vel +2.1%; accel 4.95× spec at 1 s window (compliant at ≥2.2 s)
T-02	387.19 (J1)	4731.92 (J2)	Vel 2.15× spec; accel 11.8× spec at 1 s window (compliant at ≥3.4 s)
T-03	56.26 (J1)	187.47 (J1)	Both within limits
T-04	225.37 (J1)	1992.71 (J1)	Vel 1.25× spec; accel 4.98× spec at 1 s window (compliant at ≥2.2 s)
T-05	183.84 (J1)	1743.95 (J3)	Vel +2.1%; accel 4.36× spec at 1 s window (compliant at ≥2.1 s)

Four of five movements exceeded UR5e speed limits; T-02 ran at 11.8× the limit and T-05 at 4.36× the limit. Three movements exceeded UR5e angular acceleration limits. Only one movement was feasible; therefore, both “collision-free” tests would not have functioned on the real hardware: T-03 was the only safe command that could have been sent to the UR5e hardware without any warnings. T-05 had enough angular acceleration remaining before exceeding the robot limit, so perhaps a much lower speed limit for T-02, T-03, and T-04 could have resulted in safe travel. The current results show T-05 as performing the worst, after T-02.

3.2.3. Statistical Study on Task T-01

Table 4 presents the 30-trial statistical breakdown of task T-01. In all 30 trials, the approach failed completely to generate a solution, yielding a 0% success rate. Each instance used up the entire 200-iteration limit, never reaching a collision-free trajectory, and the mean planning time was 16.79 s, exceeding the 10 s deadline. Note that the 10 s time check happened at the end of each iteration rather than in the middle of a computation, so a lengthy final iteration would push the sum of the iteration times past the 10 s boundary. The Statistical distributions of the 30 trials are shown in Figure 3.

Table 4. Thirty-trial statistical analysis on task T-01. Every trial lasted up to the full 200 iterations, and every trial failed to generate a collision-free trajectory.

Metric	Mean	Std Dev
Planning Time (s)	16.7851	±1.6882
Iterations	200.0	±0.0
Final Cost	0.004073	±0.001133
Path Length (rad)	1.7601	±0.0452
Success Rate	0.0%	(0 of 30 trials)

Figure 3. Statistical distributions of the 30 trials carried out on MATLAB (created by the researchers on MATLAB): (a) planning time distribution, showing the computational cost required for trajectory optimization to converge or terminate; (b) iteration count distribution, showing the number of optimization iterations executed per trial before convergence; (c) final cost distribution, showing the converged (or terminal) value of the CHOMP objective function across trials.

The standard deviation of the trajectory length was 0.0452 radians for a mean length of 1.7601 radians, with a coefficient of variation of about 2.6%. CHOMP, as configured, consistently produced a path nearly the length of the optimum solution for this setup, but the trajectory was consistently unworkable for task T-01. The problem here is not due to tweaking—it is a failure of CHOMP to do what is expected on this task.

3.2.4. Sensitivity Analysis

After performing a sensitivity sweep over the smoothness weight, ws, from 10⁻³ to 10¹, the algorithm took 100 iterations at each ws to find the solution to task T-01, the results of which are shown in Figure 4, subplot 5. These results appear reasonable: for low values of ws, the smoothness constraint allows flexibility in responding to obstacle proximity, resulting in higher frequencies of joint motion; for high values of ws, the resulting trajectory resembles the initialized quintic one, and thus fails to navigate the obstacles. The value we use in our experiments, ws = 0.1, is neither the minimum-cost solution nor near the minimum; the closest values are between ws = 10⁻² and 10⁻¹. Thus, the standard ws value is not ideal for this particular problem, which may warrant investigation for future real-world applications of our method.

Figure 4. (a) Chomp cost convergence, (b) optimized joint trajectories, (c) joint velocity profiles, (d) joint acceleration profiles, and (e) sensitivity smoothness weight of the CHOMP analysis of the UR5e pick-and-place configurations on task T-01 carried out in MATLAB.

3.2.5. MATLAB Chart Analysis

Figure 4 shows the T-01 diagnostic panel. The top-left cost convergence plot is the standard CHOMP diagnostic. The total cost drops from about 0.225 to 0.023 over 115 iterations, but the total cost curve (blue) is essentially indistinguishable from the obstacle cost curve (red dashed), while the smoothness cost (green) sits near zero for the entire duration. This overlap is driven almost entirely by obstacle avoidance, with the smoothness term contributing almost nothing to the optimization. The joint trajectory and velocity panels show the kinematic evidence for the jerky trajectory. Peak velocity reaches ~5 at 184°/s on J1, while on J3, it reaches near 1 at 2000°/s² and experiences some sharp and narrow spikes (normalized time 0.3, 0.45, 0.65). The “s” is defined as the jerky trajectory, where even though CHOMP claims that the smoothness of the trajectory is nearly zero, the corresponding resulting acceleration for each joint is far from smooth. This is where the novelty of adaptive CHOMP becomes apparent, as the velocity penalized by CHOMP’s quadratic smoothness functional does not necessarily mean instantaneous spike-free acceleration, given their non-linear derivative relations. Figure 5 shows the multi-task comparison, providing a summary of the five problems. It can be seen that T-01, T-02, T-04, and T-05 took almost all 10 s of the planning time to work and needed a lot of iterations (>100 (115–156)), whereas T-03 only took five iterations or 0.16s to converge. The quintic interpolation for task T-03 passes safely above all obstacles right from the beginning (see Figure 5). In T-03, CHOMP is only asked to make the path smoother.

Figure 5. Multi-task CHOMP performance of the UR5e pick-and-place configurations carried out in MATLAB.

4. Discussion

The failure modes observed here are broadly consistent with previous reports in the literature. Kalakrishnan et al. [3] (2011) were motivated to design STOMP precisely by the local minimum problem observed on T-01, T-02, and T-04. Schulman et al. [24] (2013) were motivated to design TrajOpt by the absence of hard constraint enforcement that manifests here as the kinematic limit violations. What the present study adds is a specific, reproducible, numerical characterization on a widely used robot platform under MoveIt2 default parameters, which should be useful as a reference point for other researchers developing improved optimizers. More recent work, such as the improved RRT*-Connect variant of He et al. [7], continues to refine sampling-based planners for cluttered manipulator workspaces, underscoring that the limitations documented here for optimization-based CHOMP remain an active concern across planner families [25].

This section presents the root-cause analysis, explaining why baseline CHOMP fails—due to local minimum entrapment, the formulation’s blindness to kinematic limits, and a 0% statistical success rate—and maps each failure mode directly onto the adaptive CHOMP modifications proposed to fix it.

4.1. Why Does T-03 Succeed and T-01 Fail?

In contrast to T-03, for task T-01 the initial quintic interpolation brings the manipulator into close proximity to one of the obstacles. Near the mid-point of the trajectory, the manipulator approaches the obstacle situated at coordinates (0.30, 0.20, 0.40) within a minimum distance of 0.07 m. As a consequence, the obstacle force pushes the middle of the interpolated path away from the obstacle. However, because of the clamped gradient (0.1 radians of change max per joint per iteration), the robot arm can barely move. Then, the smoothness forces pull back that part of the interpolated trajectory as the next iteration starts. Hence, the robot arm keeps wiggling around the obstacle but can never escape its vicinity. Thus, in spite of having had plenty of processing time and iterations, T-01 is not solved. The optimizer oscillates near a local minimum just above the collision threshold, which is exactly what the cost history in Figure 4 shows.

4.2. Root Cause of the Kinematic Limit Violations

The peak velocities and accelerations are computed via finite differences on the discrete trajectory. With N = 50 waypoints over a normalized time interval, dt = 1/49, and a joint displacement of order 1 rad between consecutive waypoints during the problematic regions, the implied velocity at a normalized rate is on the order of 49 rad/s, which translates directly to several thousand deg/s if the trajectory were executed in one second of wall-clock time. The key observation is that the CHOMP cost functional operates on the trajectory in a time-normalized representation and has no mechanism to ensure that the eventual time-parameterization will respect velocity or acceleration limits. Once the trajectory is executed at any plausible time scale—for example, over the 2–4 s typical of a UR5e pick-and-place motion—the resulting joint accelerations exceed the manufacturer limit by the factors reported in Table 3.

This is not a defect of the MATLAB implementation; the key observation is that the CHOMP cost functional operates on the trajectory in a time-normalized representation and has no mechanism to ensure that the eventual time-parameterization will respect velocity or acceleration limits [26].

The standard workaround is to apply post hoc time scaling via the MoveIt2 Add TimeOptimal Parametrization adapter, which rescales the trajectory duration until all kinematic constraints are satisfied. That adapter was not applied in the present study because the goal was to characterize the raw CHOMP output. The implication for the paper is that any adaptive variant intending to produce directly executable trajectories must either (a) incorporate kinematic barrier terms into the cost functional itself, in the spirit of jerk-bounded trajectory generation methods that explicitly constrain higher-order derivatives [14], or (b) be wrapped with a mandatory post-optimization time-scaling step, for which the time-optimal path parameterization is based on reachability analysis [27]. The first path has been adopted in a planned adaptive CHOMP by introducing velocity and acceleration barriers that draw inspiration from the interior-point penalty methods used by Schulman et al. [24] (2013).

4.3. Implications of the 0% Statistical Success Rate

The 30-trial results on T-01 are the most concerning result of the experiment. Minor variations in start and goal parameters should not change whether our optimizer works; on well-behaved problems, the success rates should be near 100% and robust against such variations. The success rate of 0% implies that T-01 lies in a region that is difficult to solve with this combination of obstacles, learning rate, and iteration budget. The small variance in path lengths across trials (standard deviation of 0.0452 rad and average path length of 1.76 rad) implies that the added noise of stochastic descent is insufficient to overcome this localized difficult region, and that each of the 30 initial random positions for the spline resulted in the same minimum-path solution. This suggests that one might need a more robust stochastic optimizer, such as STOMP [3], or might need to carry out a restarting procedure that can recognize stalled training, stop, and reinitialize a new random spline basis from elsewhere.

4.4. Smoothness Weight Sensitivity

The sensitivity sweep (Figure 4, subplot 5) shows that total cost is not monotonic with ws. If ws = 10⁻³, the smoothing term is negligible, but there are obstacles with arbitrary cost to the left. If ws = 10¹, the smoothing term dominates and the generated trajectory will not deform. The minimum total cost is in between, and there will be an optimal ws somewhere, which seems unlikely at ws = 0.1. MoveIt2’s default values are usable as general-purpose default parameters, but do not seem to be optimal parameters at this point. We plan to implement an adapter module that dynamically adjusts ws according to the magnitude of the observed obstacle gradient.

4.5. Implications for the Adaptive CHOMP Variant

In the literature on robotics, there is a strong incentive to only report positive successes. The work published here takes an intentionally opposite tack: it intentionally reports failure. It is not at all embarrassing to show the 0% success rate in the experiment with 30 repetitions and three task timeouts. In point of fact, these are good data points showing the weaknesses in CHOMP at a baseline that is conceptually well-known, but rarely reported to this extent in a scientific paper. It makes a more powerful argument when it comes to claims of progress on top of that baseline. Section 4 is broken down according to different failures. These failure modes provide some direct leads on exactly how to fix CHOMP through the modifications proposed in the adaptive CHOMP. Firstly, an adaptive learning rate schedule—starting from 0.01 and increasing when the obstacle gradient magnitude exceeds a threshold—should help trajectories escape the local minima observed in T-01, T-02, and T-04. Second, kinematic barrier terms penalizing velocity and acceleration beyond the UR5e manufacturer limits should be added to the cost functional, addressing the limit violations observed across four of the five tasks. Third, a stagnation-detection restart strategy should be reinitialized from a perturbed spline basis when the obstacle cost fails to decrease over a sliding window of iterations, addressing the statistical study failure. These directions are complementary to representation-level alternatives such as continuous-time Gaussian process formulations [8], against which an adaptive CHOMP variant should ultimately be compared, and to hybrid schemes that combine sampling with trajectory optimization to improve convergence [28].

4.6. Limitations of the Present Study

Several limitations should be acknowledged. First, only five task configurations were tested; a larger benchmark of at least 50 diverse tasks is required for statistically robust claims and is planned for the next experimental phase. Second, the ROS2 benchmarking comparison against OMPL RRT-Connect, the MoveIt2 CHOMP plugin, and Pilz LIN was not available during the reported run because the ROS2 stack was not active; this comparison is the subject of an ongoing experimental campaign, for which tooling such as Robowflex [29] simplifies the construction of reproducible MoveIt planning queries. Third, the obstacle model uses spheres only; real pick-and-place scenes contain boxes, cylinders, and arbitrary meshes that require different distance computations. Fourth, dynamic obstacles and replanning scenarios were not considered at all. Each of these limitations defines a follow-up experiment for the paper.

5. Conclusions

This paper has presented an empirical evaluation of the CHOMP trajectory optimizer on a UR5e manipulator using MATLAB and full forward kinematics for obstacle cost computation. The main finding is that baseline CHOMP with the standard MoveIt2 hyperparameters is inadequate for the set of pick-and-place tasks studied. Only one of five tasks produced a fast, clean, collision-free, and kinematically feasible trajectory. Two tasks reached the convergence criterion only at the edge of the time budget, two failed outright, and the 30-trial robustness study on the first task yielded a 0% success rate. Four tasks yielded paths where calculated joint accelerations outstripped the manufacturer’s limit (4.36× the minimum of five trials).

These observations form the motivation for an adaptive CHOMP variant. To this end, three key improvements will be made: (a) the utilization of adaptive learning rates (as each individual calculation for each task performed differently on average); (b) kinematic obstacle (barrier) constraints to prevent issues encountered with collision testing in three instances; and (c) the implementation of restart optimization mechanisms that are automatically triggered when stagnation in training is observed in one out of five failed tasks. The effectiveness of this method against this trajectory generator (CHOMP), along with the Movelt2 motion planning plugins, will be analyzed on an expanded data set of 50+ robot task configurations.