Analytic Collision Costs for STOMP: A Geometry-Informed Framework for Manipulator Motion Planning

Analytic Collision Costs for STOMP: A Geometry-Informed Framework for Manipulator Motion Planning Fabio Mastromarino, Raffaele Carli, and Mariagrazia Dotoli Abstract— Trajectory optimization for robotic manipulators requires balancing smoothness, feasibility, and reliable collision avoidance in cluttered environments. Classical approaches such as Covariant Hamiltonian Optimization for Motion Planning (CHOMP) and Stochastic Trajectory Optimization for Motion Planning (STOMP) have shown strong performance, but they generally depend on Signed Distance Fields (SDFs) to represent obstacle proximity. This reliance introduces geometric approximations, discontinuities, and additional computational overhead. In this work, we present a geometry-aware extension of the STOMP framework in which the robot and obstacles are modeled with differentiable primitives—cylindrical links and spherical obstacles. This formulation allows direct and accurate evaluation of collision costs, removing the need for precomputed SDFs or explicit gradient-based updates. By preserving STOMP’s stochastic optimization structure, our method overcomes the limitations of gradient-descent strategies such as CHOMP while preserving robustness and scalability. The resulting planner generates smooth, collision-free trajectories at reduced computational cost, making it well suited for highdimensional manipulators operating in complex environments. I. INTRODUCTION Trajectory planning for robotic manipulators is a fundamental problem in robotics, aimed at generating smooth, safe, and efficient motions in the presence of kinematic constraints and static or dynamic obstacles. Optimizationbased methods have proven particularly effective in this context, typically operating over discretized trajectories by minimizing cost functions that trade off smoothness and obstacle avoidance. One of the foundational approaches is Covariant Hamiltonian Optimization For Motion Planning (CHOMP) [6], which formulates motion planning as a covariant gradient-based optimization problem combining smoothness and obstacle proximity costs. However, CHOMP is sensitive to local minima and requires differentiable cost functions, which can limit its applicability in complex scenarios. To overcome these issues, Stochastic Trajectory Optimization for Motion Planning (STOMP) [1] introduces a stochastic trajectory optimization strategy that avoids the need for gradient computation by sampling and averaging trajectory perturbations. While more robust to non-convexities, STOMP still depends on SDF-based obstacle representations, inheriting the same geometric limitations. Both CHOMP and STOMP use Signed Distance Fields (SDFs) to represent obstacle costs. While effective, SDFThe authors are with the Department of Electrical and Information Engineering, Polytechnic of Bari, Italy. (email: {fabio.mastromarino, raffaele.carli, mariagrazia.dotoli}@poliba.it). based formulations typically require precomputed distance maps, may introduce geometric approximations due to discretization, and can exhibit gradient artifacts near obstacle boundaries that affect optimization performance. More broadly, trajectory optimization includes formulations such as probabilistic inference [3], sequential convex programming [5], and data-driven learning, each with tradeoffs in accuracy, scalability, and computational cost. This work addresses these limitations by proposing a geometrically-informed extension of the STOMP framework that replaces the SDF-based obstacle model with a differentiable, geometry-aware formulation that more accurately reflects the physical structure of the robot and its environment. Specifically, we represent robot links as cylinders and obstacles as spheres, allowing for efficient and accurate cost computation directly from geometric primitives. The proposed method maintains the stochastic optimization backbone of STOMP while significantly improving precision, scalability, and robustness in complex environments. II. METHODOLOGY This work proposes a geometry-aware extension of the STOMP framework [1], specifically designed to enhance collision avoidance in robotic manipulators. The objective is to compute smooth, feasible, and collision-free trajectories while preserving computational efficiency and enabling scalability to high-dimensional settings. The robot is modelled as an articulated kinematic chain composed of Nrigid links. Each link is represented as a cylinder characterised by a radius rc,i ∈R>0and a height hi∈R>0, related to the corresponding link. The pose of each cylindrical link is defined relative to a local coordinate frame Σi, whose transformation concerning the global inertial frame Σ0depends on the joint configuration θ∈Rn. The central axis of each cylinder is defined as a parametric line: Li(θ) = np∈R3p=pi,1(θ)+λˆ di(θ), λ ∈Ro,(1) where the direction vector ˆ di(θ)is computed as: ˆ di(θ) = pi,2(θ)−pi,1(θ) ∥pi,2(θ)−pi,1(θ)∥.(2) The surrounding environment is populated by static obstacles, which are modeled as spheres with fixed centers cs,j ∈R3and radii rs,j ∈R>0. This simplified yet expressive geometric abstraction enables efficient and analytically tractable distance computations, avoiding the need 2025 I-RIM Conference October 17-19, Rome, Italy ISBN: 9788894580570 10.5281/zenodo.17629904 259 for complex mesh-based collision checks or costly SDF evaluations. Several works in the literature support modelling both the robot and obstacles with spheres, exploiting the analytic nature of distance gradients between spherical primitives. [2], [4], [6], exploiting the analytic nature of distance gradients between spherical primitives. These approaches demonstrate that using sphere-based models not only reduces computational complexity but also facilitates the integration of optimization-based and learning-based planners in realtime settings. Trajectory planning is formulated as the optimization of a discretized sequence of joint configurations Θ= [θ1,...,θN]. The optimization process seeks to minimize a composite cost function that balances multiple objectives: C(Θ) = N X i=1 (wobs cobs(θi)+waux caux(θi)) + 1 2Θ⊤RΘ, (3) where the first term penalizes proximity to obstacles, the second accounts for auxiliary constraints such as joint limits or dynamic feasibility, and the final regularization term, governed by a positive semi-definite matrix R, enforces trajectory smoothness. The obstacle cost cobs is computed by summing penalty terms over selected pairs of robot links and nearby obstacles. Specifically: cobs(θi) = X (k,j)∈P(θi) ψk,j(θi),(4) where P(θi)denotes the set of relevant cylinder–sphere pairs at configuration θi, and the individual penalties are given by: ψk,j(θi) = (1 2(ϵ−dk,j(θi))2if dk,j(θi)< ϵ, 0otherwise.(5) Here, dk,j (θi)denotes the signed distance between the surface of cylinder Ckand the surface of sphere Sj, computed as: dk,j(θi)=d(k,j) ⊥(θi)−(rc,k +rs,j).(6) To avoid unnecessary computations, only those pairs that are likely to be in proximity are considered. The filtering is based on two conditions: (d(k,j) ⊥(θi)≤rc,k +rs,j, d(k,j) c(θi)≤rs,j +hk 2,(7) where d(k,j) ⊥is the orthogonal distance from the sphere center to the axis of the cylinder, and d(k,j) cis the Euclidean distance between the sphere center and the cylinder center. This twostage proximity check ensures computational efficiency by restricting evaluations to spatially relevant interactions. The optimization proceeds iteratively using the stochastic update rule of STOMP. At each iteration, a batch of M perturbed trajectories ˜ Θis generated by sampling from a multivariate normal distribution centered at the current trajectory: ˜ Θ∼ N(Θ,Σ),(8) where Σencodes the noise covariance. Each sampled trajectory is evaluated using the cost function described above, and the trajectory is then updated using a weighted average of the perturbations that lead to lower costs: Θ(k+1) =Θ(k)+ ∆Θ(k).(9) This formulation inherits the robustness and gradientfree nature of the original STOMP algorithm, allowing it to operate in non-smooth cost landscapes and complex geometric environments. At the same time, the explicit geometric modeling enhances accuracy and scalability, offering a powerful alternative to SDF-based techniques. The result is an efficient and flexible trajectory planner that better captures real-world robot-obstacle interactions, without relying on gradient descent or differentiable distance fields. III. CONCLUSIONS AND OUTLOOKS This work presents a geometry-aware extension of the STOMP trajectory optimization framework, enabling more accurate and computationally efficient collision avoidance for robotic manipulators. By explicitly modeling the robot and obstacles using differentiable primitives, We eliminate the reliance on precomputed SDFs and their associated geometric approximations. A key advantage of this formulation is that it remains fully compatible with stochastic optimization, thereby avoiding the need for explicit gradient computation required by methods such as CHOMP. This allows our approach to handle nondifferentiable cost landscapes and complex collision geometries more robustly, without sacrificing convergence or scalability. Future work will explore extensions to dynamic environments and articulated obstacles, as well as real-time implementation and integration with learning-based motion primitives to enhance planning efficiency and generalization capabilities further. REFERENCES [1] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, and S. Schaal. Stomp: Stochastic trajectory optimization for motion planning. In 2011 IEEE international conference on robotics and automation, pages 4569– 4574. IEEE, 2011. [2] J. Michaux, A. Li, Q. Chen, C. Chen, B. Zhang, and R. Vasudevan. Safe planning for articulated robots using reachability-based obstacle avoidance with spheres. arXiv preprint arXiv:2402.08857, 2024. [3] M. Mukadam, J. Dong, X. Yan, F. Dellaert, and B. Boots. Continuoustime gaussian process motion planning via probabilistic inference. The International Journal of Robotics Research, 37(11):1319–1340, 2018. [4] C. Park, J. Pan, and D. Manocha. Itomp: Incremental trajectory optimization for real-time replanning in dynamic environments. In Proceedings of the international conference on automated planning and scheduling, volume 22, pages 207–215, 2012. [5] J. Schulman, J. Ho, A. X. Lee, I. Awwal, H. Bradlow, and P. Abbeel. Finding locally optimal, collision-free trajectories with sequential convex optimization. In Robotics: science and systems, volume 9, pages 1–10. Berlin, Germany, 2013. [6] M. Zucker, N. Ratliff, A. D. Dragan, M. Pivtoraiko, M. Klingensmith, C. M. Dellin, J. A. Bagnell, and S. S. Srinivasa. Chomp: Covariant hamiltonian optimization for motion planning. The International journal of robotics research, 32(9-10):1164–1193, 2013. 260