GeoPF: Infusing Geometry into Potential Fields for Reactive Planning in Non-trivial Environments
Extracted source text · arXiv 2505.19688
\RequirePackage[reqno]{amsmath}
\documentclass[letterpaper, 10 pt, journal, twoside]{ieeetran}
\IEEEoverridecommandlockouts
\usepackage{svg}
\usepackage{svg}
\usepackage{color}
\usepackage{transparent}
\usepackage{graphicx}
\usepackage{microtype}
\usepackage{import}
\usepackage{wrapfig}
\usepackage{algorithm}
\usepackage{diagbox}
\usepackage{subcaption}
\usepackage{multicol}
\usepackage{tablefootnote}
\usepackage{cases}
\usepackage{CJK}
\usepackage{indentfirst}
\usepackage{empheq}
%\usepackage{txfonts}
%\usepackage{algpseudocode}
% \newcommand{\includesvg}[2]{%
% \def\svgwidth{#1}%
% \import{i}{#2.pdf_tex}%
\input{abkuerzung.sty}
\input{definitions/packages2Include.tex}
\input{definitions/macros.tex}
\input{definitions/colorcode.tex}
%\newcommand{\cor}[1]{\textcolor{red}{#1}}
%\newcommand{\cob}[1]{\textcolor{blue}{#1}}
\renewcommand{\todo}[1]{\textcolor{red}{\bfseries ToDo: #1}}
\begin{document}
% paper title
\title{GeoPF: Infusing Geometry into Potential Fields for Reactive Planning in Non-trivial Environments\vspace{-4pt}}
% You will get a Paper-ID when submitting a pdf file to the conference system
\author{Yuhe Gong$^{1}$, Riddhiman Laha$^{2}$, and Luis Figueredo$^{1,2,*}$\vspace{-11pt}
% List of authors
% <-this % stops a space
% \thanks{*This work was not supported by any organization}
% <-this % stops a space
% \thanks{Manuscript received: July 26, 2023; Revised October 31, 2023; Accepted December 5, 2023.}%Use only for final RAL version
\thanks{$^{1}$The authors are with the School of Computer Science, University of Nottingham, Nottingham, United Kingdom.$^{2}$The authors are with Technical University of Munich (TUM), Munich Institute of Robotics and Machine Intelligence (MIRMI), Munich, Germany. *Corresponding author.
%Email:\texttt{\{xxxxx, xxxxxx, xxxxx, xxxxx, haddadin\}@tum.de}.$^{2}$The authors are with the School of Computer Science, University of Nottingham, Nottingham, United Kingdom. Email: \texttt{xxxxx,xxxxxx@nottingham.ac.uk}. Luis Figueredo also holds an associated fellowship at the MIRMI at the Tech nical University of Munich. %
Email: \texttt{yuhe.gong@nottingham.ac.uk, riddhiman.laha@tum.de, figueredo@ieee.org}.
% This work was funded by the Lighthouse Initiative Geriatronics by StMWi Bayern (Project X, grant 5140951).
}
}
%\thanks{
% This work was supported by the Lighthouse Initiative Geriatronics by StMWi Bayern (Project X, grant 5140951), LongLeif GaPa gGmbH (Project Y, grant 5140953).} %Use only for final RAL version
% \thanks{Yuhe Gong and Luis F.C. Figueredo are with the School of Computer Science at The University of Nottingham. L.F.C. Figueredo is also an Associated Fellow at the Tech. University of Munich (TUM). The author is with Munich Institute of Robotics \& Machine Intelligence, Technische Universität München (TUM), Germany.
% This work was funded by the Lighthouse Initiative Geriatronics by StMWi Bayern (Project X, grant 5140951), LongLeif GaPa gGmbH (Project Y, grant 5140953).
% (Verbundprojektnummer V5ARA202) by the
%, I.AM (grant no. 871899).
% , and DARKO (grant no. 101017274). Please note that
% Email:\texttt{\{riddhiman.laha\}@tum.de}
% }
% \thanks{Digital Object Identifier (DOI): see top of this page.}
% }
% \markboth{IEEE Robotics and Automation Letters. Preprint Version. Accepted Month, Year}
% {Yao \MakeLowercase{\textit{et al.}}: EDM}
%\markboth{IEEE Robotics and Automation Letters. Preprint Version.}
%{Gong \MakeLowercase{\textit{et al.}}: GeoPF}
\newcommand{\revised}[1]{\textcolor{black}{#1}}
\newcommand{\minorrev}[1]{#1}
\newcommand{\removed}[1]{}
\maketitle
\begin{abstract}
% Artificial Potential Fields (APF) is still widely used in robot motion planning, since its inception in the 80's, due to its computational efficiency, ease of implementation, and real-time applicability. However, a major concern that remains is the trivial environment representation including obstacle shape and complexity when tightly integrated with a perception pipeline. To this end, encapsulation of everyday objects without being overly conservative is crucial. Therefore, in this work, we provide a fresh perspective on the construction of these force fields while taking into account the geometrical aspects. Our geometric potential field includes diverse geometric primitives in 3D space, such as the points, lines, and planes relationship, for a more efficient and precise vector field representation. Thorough simulation and real-world assessments demonstrate that the proposed geometric variant significantly outperforms the traditional potential field in terms of computational time, flexibility, and success rates.
Reactive intelligence remains one of the cornerstones of versatile robotics operating in cluttered, dynamic, and human-centred environments. Among reactive approaches, potential fields (PF) continue to be widely adopted due to their simplicity and real-time applicability. However, existing PF methods typically oversimplify environmental representations by relying on isotropic, point- or sphere-based obstacle approximations. In human-centred settings, this simplification results in overly conservative paths, cumbersome tuning, and computational overhead---even breaking real-time requirements. In response, we propose the Geometric Potential Field (GeoPF), a reactive motion-planning framework that explicitly infuses geometric primitives---points, lines, planes, cubes, and cylinders---their structure and spatial relationship in modulating the real-time repulsive response.
%into real-time planning. By leveraging precise closed-form distance functions,
% GeoPF significantly reduces computational complexity and parameter tuning effort.
Extensive quantitative analyses consistently show GeoPF's higher success rates, reduced tuning complexity (a single parameter set across experiments), and substantially lower computational costs (up to 2 orders of magnitude) compared to traditional PF methods. Real-world experiments further validate GeoPF’s reliability, robustness,
% robustness
and practical ease of deployment, as well as its scalability to whole-body avoidance. GeoPF provides a fresh perspective on reactive planning problems driving geometric-aware temporal motion generation, enabling flexible and low-latency motion planning suitable for modern robotic applications.
\end{abstract}
\begin{IEEEkeywords}
Reactive Planning, Redundant Robots, Manipulation Planning, Motion and Path Planning
\end{IEEEkeywords}
\IEEEpeerreviewmaketitle
% \renewcommand{\todo}[1]{\textcolor{red}{\bfseries ToDo: #1}}
% \title{\LARGE \bf Geometric Artificial Potential Field}
% Alternative:
% The order may change. Nico if you prefer to be the pre-last, for it is fine. I think we are not allowed to be last though :/
% \author{Yuhe Gong$^{1}$, Riddhi, Luis Figueredo
%
% Order to be defined:
% Rafael I. Cabral Muchacho
% Kim Pepper
% Elisabeth Jensen
% Luis Figueredo
% Sami Haddadin
%
% \thanks{
% \thanks{$^{1}$The authors are with the School of Computer Science, University of Nottingham, Nottingham, United Kingdom.$^{2}$The authors are with Technical University of Munich (TUM), Munich Institute of Robotics and Machine Intelligence (MIRMI), Munich, Germany.
% %Email:\texttt{\{xxxxx, xxxxxx, xxxxx, xxxxx, haddadin\}@tum.de}.$^{2}$The authors are with the School of Computer Science, University of Nottingham, Nottingham, United Kingdom. Email: \texttt{xxxxx,xxxxxx@nottingham.ac.uk}. Luis Figueredo also holds an associated fellowship at the MIRMI at the Technical University of Munich. %
% Email: \texttt{yuhe.gong@nottingham.ac.uk}.
% This work was funded by the Lighthouse Initiative Geriatronics by StMWi Bayern (Project X, grant 5140951) and supported by Nokov Motion Capture Mars.
% }
% }
% \renewcommand{\baselinestretch}{1}
% \begin{document}
% \maketitle
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% \begin{abstract}
% Artificial Potential Field (APF) is widely used in robot path planning due to the computational efficiency, ease of implementation and real-time applicability. However, currently APF always
% A safe, reactive, flexible path planning strategy in complex scenarios. The potential field method is a widely used approach that generates a force field based on the goal and surrounding obstacles, effectively describing the robot's environment. However, traditional path planning methods often rely on spherical approximations, which limits efficiency and accuracy, especially in complex scenarios. In this work, we propose geometric potential field that applies the geometric primitives in 3D space, such as the points, lines, and planes relationship, for a more efficient and precise potential field representation. Our experiments demonstrate that the geometric potential field significantly outperforms the traditional potential field in terms of computational time, flexibility and the success rate.
% \end{abstract}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INTRODUCTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\input{content/1_introduction}
\input{content/1_problem_definition}
% \input{content/1.25_related_workd}
% \input{content/1.5_math_background}
\input{content/1.75_field_generation}
%\input{content/2_methodology_line}
% \input{content/3_methodology_plane}
% \input{content/4_methodology_cubic}
% \input{content/5_methodology_cylinder}
\input{content/6_1_quantitative_analysis}
\input{content/7_conclusion}
% \input{content/6_experiment}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RELATED WORK]
% \input{content/1.5_RelatedWork}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PROBLEM STATEMENT
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%\newpage
\bibliographystyle{IEEEtran}
\bibliography{bibliography}
% Generated by IEEEtran.bst, version: 1.14 (2015/08/26)
% \begin{thebibliography}{10}
% \providecommand{\url}[1]{#1}
% \csname url@samestyle\endcsname
% \providecommand{\newblock}{\relax}
% \providecommand{\bibinfo}[2]{#2}
% \providecommand{\BIBentrySTDinterwordspacing}{\spaceskip=0pt\relax}
% \providecommand{\BIBentryALTinterwordstretchfactor}{4}
% \providecommand{\BIBentryALTinterwordspacing}{\spaceskip=\fontdimen2\font plus
% \BIBentryALTinterwordstretchfactor\fontdimen3\font minus \fontdimen4\font\relax}
% \providecommand{\BIBforeignlanguage}[2]{{%
% \expandafter\ifx\csname l@#1\endcsname\relax
% \typeout{** WARNING: IEEEtran.bst: No hyphenation pattern has been}%
% \typeout{** loaded for the language `#1'. Using the pattern for}%
% \typeout{** the default language instead.}%
% \else
% \language=\csname l@#1\endcsname
% \fi
% #2}}
% \providecommand{\BIBdecl}{\relax}
% \BIBdecl
% \bibitem{orthey2023sampling}
% A.~Orthey, C.~Chamzas, and L.~E. Kavraki, ``Sampling-based motion planning: A comparative review,'' \emph{Annual Review of Control, Robotics, and Autonomous Systems}, vol.~7, 2023.
% \bibitem{Kragic2018}
% D.~Kragic, J.~Gustafson, H.~Karaoguz, P.~Jensfelt, and R.~Krug, ``{Interactive, collaborative robots: Challenges and opportunities},'' in \emph{IJCAI International Joint Conference on Artificial Intelligence}, 2018, pp. 18--25.
% \bibitem{kappler2018real}
% D.~Kappler, F.~Meier, J.~Issac, J.~Mainprice, C.~G. Cifuentes, M.~W{\"u}thrich, V.~Berenz, S.~Schaal, N.~Ratliff, and J.~Bohg, ``Real-time perception meets reactive motion generation,'' \emph{IEEE RAL}, 2018.
% \bibitem{laha2023predictive}
% R.~Laha, M.~Becker, J.~Vorndamme, J.~Vrabel, L.~F. Figueredo, M.~A. M{\"u}ller, and S.~Haddadin, ``Predictive multi-agent-based planning and landing controller for reactive dual-arm manipulation,'' \emph{IEEE TRO}, 2023.
% \bibitem{du2010robotic}
% N.~E. Du~Toit and J.~W. Burdick, ``Robotic motion planning in dynamic, cluttered, uncertain environments,'' in \emph{2010 IEEE International Conference on Robotics and Automation}.\hskip 1em plus 0.5em minus 0.4em\relax IEEE, 2010, pp. 966--973.
% \bibitem{marcucci2024fast}
% T.~Marcucci, P.~Nobel, R.~Tedrake, and S.~Boyd, ``Fast path planning through large collections of safe boxes,'' \emph{IEEE Trans. on Robotics}, 2024.
% \bibitem{bryson2001modularity}
% J.~J. Bryson and L.~A. Stein, ``Modularity and design in reactive intelligence,'' in \emph{Int. Joint Conf. on Artificial Intelligence (IJCAI)}, 2001.
% \bibitem{DONALD1987295}
% B.~R. Donald, ``A search algorithm for motion planning with six degrees of freedom,'' \emph{Artificial Intelligence}, vol.~31, no.~3, pp. 295--353, 1987.
% \bibitem{correll2018analysis}
% N.~Correll, K.~E. Bekris, D.~Berenson, O.~Brock, A.~Causo, K.~Hauser, K.~Okada, A.~Rodriguez, J.~M. Romano, and P.~Wurman, ``Analysis and observations from the first amazon picking challenge,'' \emph{IEEE Transactions on Automation Science and Engineering}, vol.~15, no.~1, Jan. 2018.
% \bibitem{kortenkamp2016robotic}
% D.~Kortenkamp, R.~G. Simmons, and D.~Brugali, ``Robotic systems architectures and programming,'' in \emph{Springer Handbook of Robotics}.\hskip 1em plus 0.5em minus 0.4em\relax New York, NY, USA: Springer-Verlag, 2016, pp. 283--306.
% \bibitem{dobson2014sparse}
% A.~Dobson and K.~E. Bekris, ``Sparse roadmap spanners for asymptotically near-optimal motion planning,'' \emph{IJRR}, 2014.
% \bibitem{karaman2011sampling}
% S.~Karaman and E.~Frazzoli, ``Sampling-based algorithms for optimal motion planning,'' \emph{The international journal of robotics research}, 2011.
% \bibitem{likhachev2003ara}
% M.~Likhachev, G.~J. Gordon, and S.~Thrun, ``Ara*: Anytime a* with provable bounds on sub-optimality,'' \emph{Advances in neural information processing systems}, vol.~16, 2003.
% \bibitem{likhachev2005anytime}
% M.~Likhachev, ``Anytime dynamic a\^{}*: An anytime, replanning algorithm,'' in \emph{Int. Conf. on Automated Planning and Scheduling (ICAPS)}, 2005.
% \bibitem{lavalle2001randomized}
% S.~M. LaValle and J.~J. Kuffner~Jr, ``Randomized kinodynamic planning,'' \emph{The international journal of robotics research}, vol.~20, no.~5, 2001.
% \bibitem{marcucci2023motion}
% T.~Marcucci, M.~Petersen, D.~von Wrangel, and R.~Tedrake, ``Motion planning around obstacles with convex optimization,'' \emph{Science robotics}, vol.~8, no.~84, 2023.
% \bibitem{braquet2022vector}
% M.~Braquet and E.~Bakolas, ``Vector field-based collision avoidance for moving obstacles with time-varying elliptical shape,'' \emph{IFAC-PapersOnLine}, vol.~55, no.~37, pp. 587--592, 2022.
% \bibitem{Latombe1991}
% J.-C. Latombe, \emph{Potential Field Methods}.\hskip 1em plus 0.5em minus 0.4em\relax Boston,\hspace{-2pt} MA: Springer \hspace{-2pt}US, 1991.
% \bibitem{Warren1990}
% C.~Warren, ``Multiple robot path coordination using artificial potential fields,'' in \emph{Proceedings., IEEE ICRA}, 1990.
% \bibitem{koren1991potential}
% Y.~Koren, J.~Borenstein \emph{et~al.}, ``Potential field methods and their inherent limitations for mobile robot navigation.'' in \emph{ICRA}, vol.~2, 1991.
% \bibitem{singh1996real}
% L.~Singh, H.~Stephanou, and J.~Wen, ``Real-time robot motion control with circulatory fields,'' in \emph{Proceedings of IEEE International Conference on Robotics and Automation}, vol.~3.\hskip 1em plus 0.5em minus 0.4em\relax IEEE, 1996, pp. 2737--2742.
% \bibitem{haddadin2011dynamic}
% S.~Haddadin, R.~Belder, and A.~Albu-Sch{\"a}ffer, ``Dynamic motion planning for robots in partially unknown environments,'' \emph{IFAC}, 2011.
% \bibitem{becker2023motion}
% M.~Becker, J.~K{\"o}hler, S.~Haddadin, and M.~A. M{\"u}ller, ``Motion planning using reactive circular fields: A 2-d analysis of collision avoidance and goal convergence,'' \emph{IEEE TAC}, vol.~69, no.~3, pp. 1552--1567, 2023.
% \bibitem{hussein2002motion}
% A.~M. Hussein and A.~Elnagar, ``Motion planning using maxwell's equations,'' in \emph{IEEE/RSJ IROS}.\hskip 1em plus 0.5em minus 0.4em\relax IEEE, 2002.
% \bibitem{vallivaara2011magnetic}
% I.~Vallivaara, J.~Haverinen, A.~Kemppainen, and J.~R{\"o}ning, ``Magnetic field-based slam method for solving the localization problem in mobile robot floor-cleaning task,'' in \emph{ICAR}.\hskip 1em plus 0.5em minus 0.4em\relax IEEE, 2011.
% \bibitem{ataka2022magnetic}
% A.~Ataka, H.-K. Lam, and K.~Althoefer, ``Magnetic-field-inspired navigation for robots in complex and unknown environments,'' \emph{Frontiers in Robotics and AI}, vol.~9, p. 834177, 2022.
% \bibitem{xu2020real}
% J.~Xu and K.-S. Park, ``A real-time path planning algorithm for cable-driven parallel robots in dynamic environment based on artificial potential guided rrt,'' \emph{Microsystem Technologies}, vol.~26, no.~11, 2020.
% \bibitem{sang2021hybrid}
% H.~Sang, Y.~You, X.~Sun, Y.~Zhou, and F.~Liu, ``The hybrid path planning algorithm based on improved a* and artificial potential field for unmanned surface vehicle formations,'' \emph{Ocean Engineering}, vol. 223, 2021.
% \bibitem{yao2020path}
% Q.~Yao, Z.~Zheng, L.~Qi, H.~Yuan, X.~Guo, M.~Zhao, Z.~Liu, and T.~Yang, ``Path planning method with improved artificial potential field—a reinforcement learning perspective,'' \emph{IEEE access}, vol.~8, 2020.
% \bibitem{wu2016collision}
% Z.~Wu, G.~Hu, L.~Feng, J.~Wu, and S.~Liu, ``Collision avoidance for mobile robots based on artificial potential field and obstacle envelope modelling,'' \emph{Assembly Automation}, vol.~36, no.~3, pp. 318--332, 2016.
% \bibitem{radhakrishnan2024state}
% S.~Radhakrishnan and W.~Gueaieb, ``A state-of-the-art review on topology and differential geometry-based robotic path planning—part ii: planning under dynamic constraints,'' \emph{IJIRA}, vol.~8, no.~2, pp. 455--479, 2024.
% \bibitem{becker2025informed}
% M.~Becker, P.~Caspers, T.~Lilge, S.~Haddadin, and M.~A. M{\"u}ller, ``Informed circular fields: a global reactive obstacle avoidance framework for robotic manipulators,'' \emph{Frontiers in Robotics and AI}, vol.~11, 2025.
% \bibitem{shoemake1985animating}
% K.~Shoemake, ``Animating rotation with quaternion curves,'' in \emph{ACM SIGGRAPH computer graphics}, vol.~19, no.~3.\hskip 1em plus 0.5em minus 0.4em\relax ACM, 1985.
% \bibitem{laha2022coordinate}
% R.~Laha, R.~Sun, W.~Wu, D.~Mahalingam, N.~Chakraborty, L.~F. Figueredo, and S.~Haddadin, ``Coordinate invariant user-guided constrained path planning with reactive rapidly expanding plane-oriented escaping trees,'' in \emph{IEEE ICRA}, 2022.
% \bibitem{laha2021reactive}
% R.~Laha, L.~F. Figueredo, J.~Vrabel, A.~Swikir, and S.~Haddadin, ``Reactive cooperative manipulation based on set primitives and circular fields,'' in \emph{IEEE ICRA}, 2021.
% \end{thebibliography}
%\input{content/7_appendix}
\end{document}
%% LyX 2.3.4.2 created this file. For more info, see http://www.lyx.org/.
%% Do not edit unless you really know what you are doing.
% \documentclass[english,letterpaper, 10 pt, conference]{ieeeconf}
% \usepackage[T1]{fontenc}
% \usepackage[latin9]{inputenc}
% \usepackage{verbatim}
% \usepackage{babel}
% \begin{document}
% \global\long\def\vsafe{v_{\text{safe}}}%
% \global\long\def\tsafe{t_{\text{safe}}}%
% \global\long\def\safetycurve{\mathcal{S}_{\text{CG}}}%
\global\long\def\dualquaternion#1{\underline{\boldsymbol{#1}}}%
\global\long\def\quaternion#1{\boldsymbol{#1}}%
\global\long\def\dq#1{\underline{\boldsymbol{#1}}}%
\global\long\def\quat#1{\boldsymbol{#1}}%
\global\long\def\mymatrix#1{\boldsymbol{#1}}%
\global\long\def\myvec#1{\boldsymbol{#1}}%
\global\long\def\mapvec#1{\boldsymbol{#1}}%
\global\long\def\dualvector#1{\underline{\boldsymbol{#1}}}%
\global\long\def\dual{\varepsilon}%
\global\long\def\dotproduct#1{\langle#1\rangle}%
\global\long\def\norm#1{\left\Vert #1\right\Vert }%
\global\long\def\mydual#1{\underline{#1}}%
\global\long\def\hamilton#1#2{\overset{#1}{\operatorname{\mymatrix H}}\left(#2\right)}%
\global\long\def\hami#1{\overset{#1}{\operatorname{\mymatrix H}}}%
\global\long\def\tplus{\dq{{\cal T}}}%
\global\long\def\getp#1{\operatorname{\mathcal{P}}\left(#1\right)}%
\global\long\def\getd#1{\operatorname{\mathcal{D}}\left(#1\right)}%
\global\long\def\swap#1{\text{swap}\{#1\}}%
\global\long\def\imi{\hat{\imath}}%
\global\long\def\imj{\hat{\jmath}}%
\global\long\def\imk{\hat{k}}%
\global\long\def\real#1{\operatorname{\mathrm{Re}}\left(#1\right)}%
\global\long\def\imag#1{\operatorname{\mathrm{Im}}\left(#1\right)}%
\global\long\def\imvec{\boldsymbol{\imath_{m}}}%
\global\long\def\vector{\operatorname{vec}}%
\global\long\def\mathpzc#1{\fontmathpzc{#1}}%
\global\long\def\cost#1#2{\underset{\text{#2}}{\operatorname{\text{cost}}}\left(\ensuremath{#1}\right)}%
\global\long\def\diag#1{\operatorname{diag}\left(#1\right)}%
\global\long\def\setvector{\mathcal{Q}}%
\global\long\def\dqd#1{\widetilde{\dq{#1}}}%
\global\long\def\vecjoints{\myvec{\Theta}}%
\global\long\def\dqrot#1{\quat r_{#1}}%
\global\long\def\dqpos#1{\quat t_{#1}}%
\global\long\def\mathpzc#1{\fontmathpzc{#1}}%
\global\long\def\cost#1#2{\underset{\text{#2}}{\operatorname{\text{cost}}}\left(\ensuremath{#1}\right)}%
\global\long\def\closedballset{\ensuremath{\mathbb{B}}}%
\global\long\def\complexset{\ensuremath{\mathbb{C}}}%
\global\long\def\realset{\ensuremath{\mathbb{R}}}%
\global\long\def\rationalset{\ensuremath{\mathbb{Q}}}%
\global\long\def\integerset{\ensuremath{\mathbb{Z}}}%
\global\long\def\naturalset{\ensuremath{\mathbb{N}}}%
\global\long\def\SE#1{\ensuremath{SE(#1)}}%
\global\long\def\SO#1{\ensuremath{SO(#1)}}%
\global\long\def\SU#1{\ensuremath{SU(#1)}}%
\global\long\def\liealgebraSE#1{\ensuremath{\mathfrak{se}\left(#1\right)}}%
\global\long\def\liealgebraSO#1{\ensuremath{\mathfrak{so}\left(#1\right)}}%
\global\long\def\liealgebraSU#1{\ensuremath{\mathfrak{su}\left(#1\right)}}%
\global\long\def\dualmatrix#1{\left\llbracket \left\llbracket #1\right\rrbracket \right\rrbracket }%
\global\long\def\quatset{\ensuremath{\mathbb{H}}}%
\global\long\def\unitquatgroup{\ensuremath{\text{Spin}(3)}}%
\global\long\def\unitquatset{\mathcal{S}^{3}}%
\global\long\def\dualquatset{\mathbb{H}\otimes\mathbb{D}}%
%
\global\long\def\tangentSpace{\mathcal{T}_p\mathcal{M}}
\global\long\def\unitdualquatgroup{\text{Spin}(3)\ltimes\mathbb{R}^{3}}%
\global\long\def\unitdualquatset{\dq S}%
\global\long\def\kinerotation{\quat r}%
\global\long\def\kineposition{\quat p}%
\global\long\def\errorrotation{\quat e_{o}}%
\global\long\def\errorposition{\quat e_{t}}%
% \end{document}
\newcommand{\dqvar}{D}
\newcommand{\qvar}{d}
% \newcommand{\joints}{\textbf{q}}
% \newcommand{\realset}{\mathbb{R}}
% \newcommand{\symgroup}[1]{\mathrm{Sym}^#1}
% \newcommand{\symgroup}[1]{{\boldsymbol{\mathcal{S}}}^{#1}}
% \newcommand{\sogroup}{\mathrm{SO(3)}}
% \newcommand{\spdmanifold}[1]{{\boldsymbol{\mathcal{S}}}_{++}^{#1}}
% \newcommand{\spdgroup}[1]{{\boldsymbol{\mathcal{S}}}_{++}^{#1}}
%---------------------------------
% SPD Variables
%---------------------------------
\newcommand{\symgroup}[1]{{\bm{\mathcal{S}}}^{#1}}
\newcommand{\sogroup}{\mathrm{SO(3)}}
\newcommand{\spdmanifold}[1]{{\bm{\mathcal{S}}}_{++}^{#1}}
\newcommand{\spdgroup}[1]{{\bm{\mathcal{S}}}_{++}^{#1}}
\newcommand{\tangentspace}{\mathcal{T}}
%---------------------------------
% Manipulability Variables
%---------------------------------
\newcommand{\manipsymb}{{\bm{M}}}
\newcommand{\manip}[1]{{\bm{{M}}}^{#1}}
% \newcommand{\manipdomain}[1]{{\boldsymbol{\mathcal{M}}}^{#1}}
\newcommand{\manipdomain}[1]{{\bm{\mathcal{M}}}^{#1}}
% \newcommand{\midbar}[1]{\stretchleftright{\langle}{#1}{\rangle}}
\newcommand{\manipref}{\bar{\bm{M}}}
\newcommand{\indexfeas}{\text{\tiny$\mathcal{F}$}}
% Manipulability Distance Computation
%---------------------------------
\newcommand{\dist}{d}
%---------------------------------
% Human and Robot Indexes
%---------------------------------
% \newcommand{\indexhuman}{h}
% \newcommand{\indexhuman}{\text{h}}
\newcommand{\indexhuman}{{\tiny \text{H}}}
% \newcommand{\indexrobot}{r}
\newcommand{\indexrobot}{{\tiny \text{R}}}
% \newcommand{\indexrobot}{\text{r}}
% \Phi_{\manipref^{\indexhuman} \smallrightarrow \manipref^{\indexrobot}}
% \newcommand{\realset}{\mathbb{R}}
% % \newcommand{\symgroup}[1]{\mathrm{Sym}^#1}
% \newcommand{\symgroup}[1]{{\boldsymbol{\mathcal{S}}}^{#1}}
% \newcommand{\sogroup}{\mathrm{SO(3)}}
% \newcommand{\spdmanifold}[1]{{\boldsymbol{\mathcal{S}}}_{++}^{#1}}
% \newcommand{\spdgroup}[1]{{\boldsymbol{\mathcal{S}}}_{++}^{#1}}
% Tangent Space of human and robot
\newcommand{\humantangentSpace}{\bm{\tangentspace}_{\tiny{\manipref^{\indexhuman}}}\manipdomain{\indexhuman}}
\newcommand{\robottangentSpace}{\bm{\tangentspace}_{\tiny{\manipref^{\indexrobot}}}\manipdomain{\indexrobot}}
\newcommand\toExp{\mathrel{\stackrel{\makebox[0pt]{\mbox{\normalfont\tiny \textbf{Exp}}}}{\to}}}
\newcommand\toLog{\mathrel{\stackrel{\makebox[0pt]{\mbox{\normalfont\tiny \textbf{Log}}}}{\to}}}
\newcommand\toPT{\mathrel{\stackrel{\makebox[0pt]{\mbox{\normalfont\tiny $\bm{\Gamma}$}}}{\to}}}
%---------------------------------
% Mapping functions
%---------------------------------
% Manipulability IK Mapping
% \newcommand{\manipIK}{{_\scriptsize{\mathcal{M}}}\text{IK}}
% \newcommand{\manipIK}{{\phantom{x}_\scriptsize{\mathcal{M}}}\text{IK}}
% \newcommand{\manipIK}{{{~}_\scriptsize{\mathcal{M}}}\text{IK}}
\newcommand{\manipIK}{\textsubscript{\footnotesize{$\mathcal{M}$}}\negthinspace\text{IK}}
%
% Parallel Transport
\newcommand{\tinyrightarrow}{\scaleto{\rightarrow}{2.5pt}}
\newcommand{\pt}[2]{\Phi_{#1 \tinyrightarrow #2}}
%
% Manipulability Scaling Factor
\newcommand{\scalingM}{\lambda_{M}}
\newcommand{\scalingIK}{\lambda_{IK}}
%
% Manipulability Domain Mapping
\newcommand{\mappingDomain}{f_{\tiny \mathcal{M}}}
%---------------------------------
% Control Variables
%---------------------------------
\newcommand{\slackqp}{\varepsilon}
%
\newcommand{\joints}{\myvec q}
\newcommand{\jointsctrl}{{\myvec u}}
% \newcommand{\jointsctrl}{\dot{\myvec{q}}^{\indexrobot}}
\newcommand{\ikcontrolfull}{\text{purely hierarchical pseudo-inverse-based controllers}}
\newcommand{\ikcontrol}{\text{Pinv-ME-Jacob}}
\newcommand{\dqmapvectorDomain}{\Psi}
\newcommand{\dqmapvector}{\psi}
\newcommand{\perspect}[1]{%
\textit{\textcolor{orange}{$#1$}}%
}
\newcommand{\distancesphere}{d^{S}}
\newcommand{\robotpoint}{\point^R}
\newcommand{\myvecx}[1]{\boldsymbol{\underline{#1}}}
% % Macro for CT per step
% \newcommand{\ctperstep}[4]{%
% \fpeval{round(#1/#3,5)} /\textcolor{gray}{\fpeval{round((#1/#3)*sqrt((#2/#1)^2 + (#4/#3)^2),5)}}
% }
% % CT per step in milliseconds (mean/std)
% \newcommand{\ctperstepms}[4]{%
% \fpeval{round(1000*#1/#3,3)}~ms/\textcolor{gray}{\fpeval{round(1000*(#1/#3)*sqrt((#2/#1)^2 + (#4/#3)^2),3)}}
% }
% Macro for CT per step in milliseconds:
% Removes "ms" in the cells, only shows in column header.
\newcommand{\ctperstepms}[4]{%
\fpeval{round(1000*#1/#3,4)} /\textcolor{gray}{\fpeval{round((1000*#1/#3)*sqrt((#2/#1)^2 + (#4/#3)^2),4)}}
}
% Macro to round min/avg distances to three decimals, remove leading zero
\newcommand{\distfmt}[1]{%
\begingroup
\fpset{\tmp}{round(#1,3)}%
\fpiflt{\tmp}{1}{.\tmp}{\tmp}%
\endgroup
}
%
% For comments
%
\newcommand{\luis}[1]{{\textcolor{blue!70}{#1}}}
\newcommand{\lf}[1]{{\textcolor{blue!70!red!60}{\textbf{LF:} #1}}}
\newcommand{\riddhi}[1]{{\textcolor{green!70!black!100}{#1}}}
\newcommand{\yuhe}[1]{{\textcolor{red!80!green!80}{#1}}}
% \newcommand{\tofullversion}[1]{#1}
\newcommand{\tofullversion}[1]{}
% \newcommand{\changetext}[1]{{\textcolor{red!90}{#1}}}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Introduction}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %Robots are getting more and more into human activities and sharing human workspace. Reactive robot behavior generation is necessary in such human-centered environments. Robots must be capable of adapting their behavior fast and effectively in cluttered and dynamic environment. A reactive robot system that avoids objects and responses to the dynamic changes helps the robot getting involved into human daily tasks [...(cite about reactive robot system)], such as pouring the tea and preparing the breakfast (Fig.~\ref{fig:teaser}). Thus, a concrete, reliable environment model with spatial and temporal information is important. Although current research focuses on the discretization [...] and randomization [...] of the environment, as well as optimization through artificial intelligence [...] and sampling-based methods [...], the increasing complexity of these systems results in expensive computation and an over-complicated models.
% % ------------------------------------------------------------
% Reactive intelligence remains the cornerstone of truly versatile robotic systems operating in complex human-centered environments~\cite{bryson2001modularity}. These systems must be capable of online behavior adjustment when subjected to cluttered, dynamic environments. Thus, for complete integration in human households, a reactive robot system that avoids collisions and responds to dynamic changes is imperative~\cite{laha2023predictive}. As an example, consider the tasks shown in Fig.~\ref{fig:teaser}, such as pouring tea, preparing the table for breakfast, moving daily objects in a human-friendly fashion. In all of these sub-tasks, a concrete and stable environment representation with spatial and temporal information is needed. Current and past research have focused on (continuous) operational space planning such as Voronoi diagrams~\cite{wen2024g}, navigation functions~\cite{rimon1992exact}, APF or (discrete) configuration space (C space) planning like RRTs, PRMs, ESTs. Although valid plans are obtained employing the above approaches, the robot might not be successful in executing them in non-trivial environments due to poor scene representation or control constraints. However, a method with a precise (online) representation of the environment and reactive planning performance remains yet to be discovered.
% % ------------------------------------------------------------
Robotics has expanded beyond structured, well-defined factory floors into (i) unstructured, (ii) cluttered, and (iii) dynamic human-centred spaces, increasing the demand for reactive intelligent navigation, Fig.~\ref{fig:teaser}.
% , as illustrated in Fig.~\ref{fig:teaser}.
% As robots increasingly share workspaces with humans, the need for reactive intelligent navigation solutions becomes paramount.
Yet, despite significant advances in motion planning \cite{orthey2023sampling}, autonomous reactive behavior %in settings (i)-(iii)
remains an open problem
\cite{kappler2018real,laha2023predictive}.
% \cite{Kragic2018,kappler2018real,laha2023predictive}.
The core challenge lies in
% balancing
% two competing demands: efficiently
\revised{addressing clutter while providing structured environment representations that enable}
%
%modeling unstructured and cluttered environments (i)-(ii) while ensuring
real-time reactivity to dynamic, unforeseen changes~\cite{du2010robotic,marcucci2024fast}.
% (iii)~\cite{du2010robotic,marcucci2024fast}.
This challenge is exemplified in Fig.~\ref{fig:teaser}. %where a
A robot must navigate a constrained environment to pour tea, set a %breakfast
table, and dynamically respond to a human moving objects in real-time.
% Achieving autonomy and in such scenarios
% Achieving reactive intelligence
%
This
requires a structured %and stable
environment representation---\revised{one capable of seamlessly integrating geometric, spatial, and temporal information into real-time reactive planning.}
% one that
% captures both spatial and temporal information---with a mapping that seamlessly integrates this knowledge into reactive motion generation alongside control constraints and real-time actions.
This work addresses precisely this open challenge by introducing %providing
\revised{a novel paradigm that explicitly leverages shape-awareness, geometric structure, and spatial relationship into repulsive strategies beyond single-gradient avoidance---enabling}
% a novel solution for
% leverages geometric semantics to significantly advance robotic autonomy in non-trivial, human-centric environments.
reactive intelligence, the centerpiece of truly versatile systems operating in human-centered non-trivial environments \cite{bryson2001modularity}.
% Classic monolithic Sense-Plan-Act (SPA) structures separate the system to sense and train, plan, act in three stages~\cite{orthey2023sampling}. This strategy enables robots to operate in structured environments by sequentially sensing the surroundings~\cite{DONALD1987295} and response to uncertain scenarios~\cite{brooks1986robust}. However, traditional SPA structures (such as the C planners mentioned earlier) face significant challenges in real-time adaptability due to its time-consuming computation, particularly in long-horizon tasks and poor converge performance in partially known environments. The SPA-planners can be further subdivided into graph (or road map-based) planners and tree-based planners. Graph-based planners utilize a sampling strategy in the free C space to build up a graph, ultimately to join the neighboring edges using a local planner. Examples of this kind include sparse roadmaps~\cite{dobson2014sparse} and PRMs~\cite{karaman2011sampling}. Tradeoffs for optimality (at the expense of scalability) using a heuristic function results in the well-known A$^{\ast}$ planner. As for the tree-based single-query planners, a sampling function is exploited to choose a random sample to grow a tree which eventually connects to the goal (repeatedly checking for collisions in between). Nonetheless, providing real-time feedback at the perception and motion generation level is non-trivial with SPA-style methods.
% %Therefore, traditional SPA framework does not suitable for the fast development in robotics. We are more interested in a framework that integrates precise environment modeling capabilities and rapid adaptability in dynamic and complex scenarios.
Traditional monolithic Sense-Plan-Act (SPA) architectures divide the system into sequential sensing, planning, and action phases \cite{orthey2023sampling}. This strong modularization enables robots to function effectively in structured, pre-modeled environments \cite{DONALD1987295,correll2018analysis,kortenkamp2016robotic}. The key advantage %of this paradigm
lies in breaking down complex tasks into manageable subproblems, making planning more intuitive and computationally feasible \cite{kappler2018real}. Most successful approaches including graph-based planners
%, such as Sparse Roadmaps \cite{dobson2014sparse} and Probabilistic Roadmaps (PRMs)\cite{karaman2011sampling},
construct a free configuration space (C-space) representation and connect edges using local planners \cite{orthey2023sampling,lavalle2001randomized,kortenkamp2016robotic}.
% \cite{dobson2014sparse,likhachev2003ara}.
% , often trading optimality for computational efficiency (e.g., A* \cite{likhachev2003ara,likhachev2005anytime}).
% In contrast
Instead, tree-based single-query planners, %such
as Rapidly-exploring Random Trees (RRTs), iteratively expand toward the goal through
% while verifying
collision-free paths \cite{lavalle2001randomized}. Despite their effectiveness in structured settings,
% these methods
they
lack real-time adaptability. Indeed, %while
SPA methods excel in handling unstructured (i) and cluttered (ii) scenarios, but they fail to adapt efficiently to dynamic (iii) changes \cite{kappler2018real,laha2023predictive}. Their inherent sequential nature hinders real-time feedback, making them ill-suited for scenarios where reactive behavior and continuous adaptation to environmental changes are critical \cite{marcucci2023motion}.
\begin{figure}[t]
\centering
\includegraphics[width=0.235\textwidth]{figs/robot_pouring_water_back_compressed.pdf}
\includegraphics[width=0.235\textwidth]{figs/robot_pouring_water_back_compressed.pdf}
\caption{Reactive robot behaviour in unstructured, cluttered, dynamic environments. %\textbf{Left}:
The robot transfers and pours the tea from the shelf to the cup (\textit{left}), %into drinking cup on the cluttered table.
% \textbf{Right}: Return back to lower shelf with avoiding a moving long kettle.
then returns the set while reacting to a human dynamically moving objects (\textit{right})---requiring accurate geometric representations for real-time motion adaptation.
}
\label{fig:teaser}
\end{figure}
% ------------------------
%
% The prevalent use of particle-based (or sphere-based) representations introduces trade-offs between accuracy and computational cost. Representing structured environments like tables with repulsive spheres requires either a highly conservative bounding sphere—leading to inefficient space usage—or numerous smaller spheres, significantly increasing computational complexity and rendering real-time performance infeasible. Conversely, using fewer sample points results in discontinuities, switching forces, and short-sighted environment representations, leading to suboptimal or even infeasible trajectories that depend on complex tuning parameters.
%
% ------------------------
In contrast, vector-field-based motion generation, or reactive planning,
% often termed control-based planning,
\revised{efficiently addresses the real-time responsiveness challenge (iii).}
% enables rapid adaptation to environmental changes \cite{braquet2022vector}, offering an efficient solution to reactivity (iii).
These methods %often
use gradient-based techniques to guide motion generation \cite{Latombe1991,Warren1990,koren1991potential}. %, ensuring real-time responsiveness.
% In human-centric environments, reactive planning has gained traction for its ability to provide real-time responses.
While early implementations suffered from local minima, %e.g., artificial potential fields (APFs),
these have been mitigated by %force-based
strategies such as circular fields (CFs) \cite{singh1996real,haddadin2011dynamic,becker2023motion,laha2023predictive} and magnetic fields \cite{hussein2002motion,vallivaara2011magnetic,ataka2022magnetic}, \revised{which incorporated the sphere geometry to generate tangential repulsive forces to complement the standard repulsive gradient}.
%
\revised{While mitigating traditional drawbacks (e.g., local minima), these methods fundamentally rely on point or sphere-based obstacle representations.}
\revised{This contrasts with modern simulators, engines and perception algorithms for collision detection (see e.g. MuJoCo \cite{todorov2012mujoco}, GJK \cite{montaut2024gjk++}, and FCL \cite{pan2015hppfcl}) %, and libccd \cite{fiser_libccd})
that compute (analytically or iteratively) contact-point distances and normals from richer geometric information.}
%
\revised{Likewise, voxel- and Signed Distance Function (SDF)-based methods \cite{oleynikova2017voxblox,curobo_icra23,bien2024generating}
% \cite{oleynikova2017voxblox,han2019fiesta,curobo_icra23,bien2024generating}
% such as
% %
% Voxblox~\cite{oleynikova2017voxblox}, FIESTA~\cite{han2019fiesta}, and CuRobo~\cite{curobo_icra23}
(as well as neural SDFs variants \cite{staroverov2024dynamic})
%
compute geometry-aware closest-point smooth gradients for collision avoidance.
% have shown the potential of SDFs to encode detailed geometric information to efficiently compute local gradients for reactive avoidance
}
%
\revised{Notwithstanding, during motion generation, all the aforementioned techniques reduce the obstacle interaction to a scalar distance with a repulsive to the closest point alone \cite{eckhoff2023towards} or local gradients \cite{bien2024generating} with respect to the closest point. That is, the rich information about the obstacle geometry, structure and spatial configuration is only used to compute the closest point and then neglected during motion generation.
%
% ---despite having explored convex primitives \cite{eckhoff2023towards} or designed or learned efficient SDFs \cite{bien2024generating}---%
% % \cite{oleynikova2017voxblox,han2019fiesta,alhaddad2024neural,staroverov2024dynamic,tzathas20233d}, works
% remain limited to scalar distance, point-contact or distance gradient computations---hindering even solutions such as CFs, as the planning would still be restricted to a closest-point distance computation alone.
}
% While geometric primitives are widely used in simulation engines and motion planning frameworks (e.g., MuJoCo, Bullet, FCL, CHOMP, TrajOpt), their usage remains limited to scalar distance or point-contact computations. In contrast, GeoPF systematically leverages the geometry and semantics of primitives — such as faces, edges, and orientations — to define structured repulsive vector fields for motion planning. This geometric modulation, introduced here for the first time in a planning framework, enables spatially-aware, interpretable, and deterministic behavior even in unstructured environments.
%
% Furthermore, hybrid approaches have attempted to integrate C-space planners with vector-field strategies, leveraging an initial trajectory while benefiting from the reactivity of field-based methods \cite{xu2020real,sang2021hybrid}.
% %
% Learning-based approaches have also been explored to enhance field-based planners~\cite{yao2020path}. However, all these methods still introduce additional computational complexity without resolving the core bottleneck of field-based methods---their reliance on simplified environment representation.
% Even with accurate geometric distance computation~\cite{24,25,28,30}, current planners typically ignore whether a contact occurs on a face, edge, or corner—treating all interactions uniformly and limiting the exploitation of geometric structure.
\revised{In other words, despite }
% Despite
their computational efficiency compared to SPA,
%these methods
\revised{existing reactive planners remain fundamentally constrained to a single-point or single-gradient, towards the closest point, interaction---disregarding the richer geometry and structural information inherent to the object.}
%
While intuitive, this particle-based approach \revised{provides poor information for the motion generation strategy. When considering point-clouds or point-based geometry, these methods lead to either large repulsive spheres yielding overly conservative plans, or dense sampling, which increases computational load}, making real-time execution impractical. Conversely, sparse sampling causes force discontinuities, switching strategies, and short-sighted environment representations, ultimately leading to suboptimal or infeasible trajectories that are highly sensitive to tuning parameters.
%
%
%
% this approach poorly represents complex objects like shelves, forcing a trade-off between large repulsive spheres resulting in overly conservative planning, whereas multiple smaller spheres, ellipsoids, or point clouds lead to excessive distance computations, making real-time execution impractical.
% Conversely, sparsely selecting sample points causes force discontinuities, complex switching strategies, and short-sighted environment representations, ultimately leading to suboptimal or infeasible trajectories that are highly sensitive to tuning parameters.
%v
% \revised{
% Even with accurate geometric-aware distance computation \cite{todorov2012mujoco,montaut2024gjk++,pan2015hppfcl,oleynikova2017voxblox,han2019fiesta,curobo_icra23,bien2024generating}, that reduce the computational load,
% %
% existing reactive predominantly still reduce obstacles to a single pointwise gradient, using the nearest-distance vector regardless of whether the closest-contact lies on a face, an edge, or a corner, for instance.}
% %
%
% rely on oversimplified workspace representations. Inspired by physics, they often model their interaction via particle-based force dynamics, which assumes point/spherical obstacle representations. While intuitive, this approach poorly represents complex objects like shelves, forcing a trade-off: large repulsive spheres result in overly conservative planning, whereas multiple smaller spheres, ellipsoids, or point clouds lead to excessive distance computations, making real-time execution impractical. Conversely, sparsely selecting sample points causes force discontinuities, complex switching strategies, and short-sighted environment representations, ultimately leading to suboptimal or infeasible trajectories that are highly sensitive to tuning parameters.
% ophisticated geometric representations and accurate distance calculations, current reactive planning methods remain fundamentally limited: they invariably reduce geometry to a single closest-point interaction, disregarding the richer semantic and structural information inherent to object geometry.
In summary, despite the promise of field-based planners in providing intuitive and reactive behaviours, their application %to real-world robotic manipulation
remains constrained by three fundamental issues: (i) high computational costs associated with representing large, non-isotropic vector fields; (ii) limited performance due to the reliance on isotropic,% and
\revised{or closest-point-based distance fields and gradients;}
% point-based distance fields;
and (iii) the complex and impractical tuning requirements necessary to handle real-world scenarios effectively.
%
%
%
%
These shortcomings are inherently tied to the environment representation upon which existing field-based methods are designed. In most cases,
\revised{repulsive forces are build upon }
% obstacles are represented using
point clouds \revised{based distances} \cite{becker2023motion,laha2023predictive}, or through spheres or octomaps \cite{braquet2022vector,radhakrishnan2024state}, leading to challenges in handling non-spherical objects and non-isotropic vector fields.
\revised{
% Even with accurate geometric-aware distance computation \cite{todorov2012mujoco,montaut2024gjk++,pan2015hppfcl,oleynikova2017voxblox,han2019fiesta,curobo_icra23,bien2024generating}, that reduce the computational load,
%
While geometric-aware distance computation methods, e.g., \cite{todorov2012mujoco,montaut2024gjk++,pan2015hppfcl,oleynikova2017voxblox,curobo_icra23,bien2024generating}
% \cite{todorov2012mujoco,montaut2024gjk++,pan2015hppfcl,oleynikova2017voxblox,han2019fiesta,curobo_icra23,bien2024generating}
help reduce the computational load (i), the resulting
%
% existing
reactive planners %predominantly
still ultimately reduce obstacles interactions to a single pointwise gradient, using the nearest-distance vector irrespective of whether the closest-contact lies on a face, an edge, or a corner.}
%
\revised{As a result, planners must rely on local contact information only, which limits geometric expressiveness and leads to non-intuitive force profiles. This lack of structural awareness often requires }
% This results in
% %in either excessive computational overhead or complex,
non-intuitive and cumbersome tuning %to handle
% \revised{the sparse obstacle information due to the access to only local information about the closest-contact point---most often}
% \revised{mostly due solely local information about the closest-contact point}
% sparse and changing obstacle representation---both cases
leading to poor or even infeasible trajectory generation.
\noindent\textbf{Contributions}
%In this work, we take a fundamentally different approach to reactive planning by casting geometric primitives for improved environment representation into force generation.
% While geometric primitives have been successfully explored in control-based methods and for constrained manipulation, e.g., surgery or multi-arm cosntraints , see e.g., \cite{xxx}, they have never been explored for reactive plannning strategies.
%Instead of focusing on incremental improvements to field-based methods---refining point-based techniques, improving tuning, or modifying isotropic fields---we propose a geometric-based representation that explicitly incorporates the spatial properties of objects into the temporal motion generation process.
\revised{The key novelty of this work lies in exploiting the rich structure of geometric primitives to directly shape the reactive vector field. %that guides the robot motion.
%
Rather than relying solely on the closest-point distance,
GeoPF leverages the structural representation of each primitive.
%
% In this context, GeoPF leverages not only the computed distance but also the structural representation of the primitive.
These features---promptly available in our method and other
geometric modeling pipelines---are most often overlooked during the planning stage.
%
Similarly, in motion optimization frameworks,
focus lies not on the shape itself but on how far the robot is from violating a closest-point contact-based threshold.
% as what essentially matters is not the shape itself but how far the robot is from violating a closest-point contact-based threshold.
Thus, local point-based gradients dictate the collision logic instead of distinguishing between the robot approaching a face, corner, or a cylindrical wall. To the best of our knowledge, such information indeed has never been used during reactive planning and field generation.}
% These features—readily available in our method and in other geometric modeling pipelines—are typically overlooked during the planning stage. Similarly, in motion optimization frameworks, the focus lies not on the shape itself but on how far the robot is from violating a closest-point contact-based threshold. As a result, collision logic is governed by local point-based gradients, without distinguishing whether the robot is approaching a face, a corner, or a cylindrical wall.
% To the best of our knowledge, no prior work has systematically incorporated this structural information into reactive vector field generation. GeoPF introduces a new paradigm that leverages this semantic richness to produce structured, interpretable, and geometry-aware repulsion behaviors during real-time planning.
Our key contributions include: \revised{%
\textbf{(C1)} A novel geometric-aware planning paradigm, revising and generalizing primitives, leveraging their topological and structural properties to actively modulate the repulsive field beyond simple distance-based computations;
%
\textbf{(C2)} An analytical, closed-form vector-field planner explicitly tailored for common primitives (points, lines, planes, cubes, and cylinders), directly instantiating C1;
%
\textbf{(C3)} Comprehensive quantitative analysis highlighting GeoPF's higher success rates and orders-of-magnitude computational improvements over state-of-the-art field-based planners, accompanied by real-world experiments --- including both whole-body avoidance and poor/coarse primitive shaping showcasing its scalability and robustness.
}
%
% \textcolor{blue}{\textbf{(C1)} A novel geometric-aware environment representation for collision avoidance, leveraging fundamental geometric primitives (points, lines, planes, cubes, and cylinders) to define the environment and calculating the distance between the robot and the geometry of each obstacles; \textbf{(C2)}
% The design of a reactive planning strategy, the Geometric Potential Field (GeoPF), based on geometric distances, which surpasses existing methods in computational efficiency, trajectory feasibility, and robustness while requiring minimal tuning; \textbf{(C3)}
% Extensive quantitative comparisons and real-world experiments, demonstrating that GeoPF achieves orders-of-magnitude improvements in computational speed over state-of-the-art reactive field-based planners while maintaining higher success rates.}
% and superior trajectory quality.}
% Vector (velocity) fields, in contrast, provide fast reactive behaviors and high robustness to complex environment changes~\cite{braquet2022vector}. Herein, the local motion generation strategy is based on gradients~\cite{Latombe1991,Warren1990,koren1991potential} with the possibility to scale up to robot manipulators~\cite{kim1992}. More recently, the growing human-centric use-cases demand real-time robot responses which is realizable using velocity fields. Past research has already demonstrated that force generation strategies such as circular field~\cite{singh1996real,haddadin2011dynamic,becker2023motion,laha2023predictive} and magnetic field~\cite{hussein2002motion,vallivaara2011magnetic,ataka2022magnetic} can be put into practice in non-stationary scenarios. However, these field generation methods exhibit inherent limitations in specific dynamic environments~\cite{duhe2021contributions}, constraining their applicability to diverse scenarios, particularly in human-centered manipulation tasks. On the other hand, sampling-based control has demonstrated improvements in optimization performance. \cite{xu2020real} introduces RRT into APF to apply random disturbance to reduce oscillations and improve time efficiency. \cite{sang2021hybrid} combines A* and APF for global optimization. Furthermore, learning-based APF methods have gained increasing attention in recent years, as they offer the potential to address uncertainties and construct precise environmental models in complex scenarios~\cite{xu2024apf,ding2023path,yuan2022uav,li2021path,lin2022apf}. However, these methods often suffer from significantly increased computational complexity and convergence problem due to the highly intricate models introduced by learning strategies, further limiting their real-time applicability. Therefore, a simple field generation approach, combined with precise environmental modeling, warrants further investigation.
% % \begin{figure}[t]
% % \centering
% % \includegraphics[width=0.235\textwidth]{figs/robot_pouring_water.pdf}
% % \includegraphics[width=0.235\textwidth]{figs/robot_pouring_water_back.pdf}
% % \caption{\textbf{Left}: Pour tea into drinking cup on the table. \textbf{Right}: Return back to lower shelf with avoiding a moving long kettle. }
% % \label{fig:teaser}
% % \end{figure}
% Real-time sensing relies on the visual feedback from sensors and cameras~\cite{kappler2018real}. The visual feedback promptly captures environmental changes and demonstrates high robustness to uncertainties. However, current research reaches a blockage which is the integration with perception and, even more critical, the modeling of the world. Most of the systems, compute the reaction behavior by computing and integrating distances to RGBD generated point-clouds [cite-Marvin-works], or modeling the world through octompas or spherization and discretization~\cite{wu2016collision,braquet2022vector,radhakrishnan2024state}. Such approaches are limited in the investigation of point-to-sphere or point-to-elliptic representations. For non-spherical objects, they needs approximation methods to model obstacles. However, the approximation results in significant computational overhead, especially when dealing with large obstacles such as walls, tables, and similar structures. One of the efficient way to avoid large approximation is the geometric models such as geometric primitives. However, even the geometric models and representation are proven to be significant useful in~\cite{marinho2019dynamic,Zhou2023}, they are rarely investigated in reactive control-based motion generation.
% \textbf{Contributions.} In this work, We leave out the over-complicated model, simply applying distance-generated APF, combied with the geometric representation of the relationships between lines and points. This approach is then extended to geometric APF (GeoAPF), representing planes, cubes, and cylinders to cover most of the common objects in human-centered environment. Our contributions are threefold: (i) Geometric representation of the obstacles with spatial relationships between the lines, planes and different 3D objects in a complex environment, (ii) motion control tracking goals under robot workspace constraints, (iii) real-time motion generation with fast reactive to dynamic and complex scenarios with outstanding performance and computational efficiency. We demonstrate that our method outperforms traditional APF and circular field methods in both static and dynamic environments. We evaluate the performance in both 3D and 2D space tasks. Quantitative and real robot experiments show that GeoAPF promises orders of magnitude reduction in computational time compared to SOTA reactive field based planners as well as improved performance and success rate.
% ~\cite{riddhi2024CF}.
% use More recently, given the necessity of more reactive behavior, multiple works have come out
% lanning actions based on a predefined model, and executing the planned motion through actuators. Existing SPA implementations typically rely on model-based approaches, ensuring predictable and deterministic behavior. However, classical monolithic SPA structures often struggle with real-time adaptability and dynamic environments, as they assume a well-defined world model and deterministic planning. In this work, we explore the limitations of traditional SPA and investigate approaches to enhance its flexibility and responsiveness.
% The reactive robot system is based on monolithic sense-plan-act structures. One type of the sense-plan-act is the sampling based methods (SBP), which sensing the surrounding environment by sampling the configuration space [Sampling-Based Robot Motion Planning: A Review]
% typically use a sequence of sensing, planning and execution process in the whole environment [...]. However, it assumes a static or predictable environment, which does not suit the demand for complex system. Sampling-based methods such as Rapidly-exploring Random Trees (RRT) [...] and Probabilistic Roadmaps (PRM) [...], A* [...] randomly samples within the workspace and find a feasible solution under constraints. However, the randomization is time-consuming and somtimes fail in real-time applications.
% The geometric modeling and representation are proven to be
% is rarely investigated in reactive dynamic and complex applications. Although geometric primitives (GPs) have been demonstrated to be beneficial in various robotic applications, a gap remains to integrate GPs into reactive robot systems.
% provide grasps the environment information according to solutions
% A reactive motion generation system enables robots to quickly adapt to the objects and changes in the environment. However,
% Path planning is one of the most popular topics in robotic research. In human-centered environment, a path planner is expected to help human daily manipulation tasks, such as pouring water, and so on. T Despite the basic features, more requirements are expected, such as the safety, computational complexity, and so on. Since robots are getting closer in human daily life, path planning is necessary in cluttering environment, diverse tasks, and afety guarantee.
% Currently, path planning includes several branches, such as sampling-based method [RRT..], neural network methods, and artificial potential methods (APF). Among them, sampling-based methods usually generate the path through the sampling of the environment, which increases the difficulty in cluttering scenarios, especially the local minimum problem and the computational wastes [...]. The neural network methods, however, are black-box methods that usually rely on the error and trial, which brings no guarantee under the scenarios with different number and size of objects [...]. Compared to them, APF attracts large attention due to its fast, simplicity and efficiency in recent year [Local path planning of intelligent vehicle based on improved artificial potential field]. APF regards the obejcts in the environments as a force generator, which can generate repulsive force to against the robot getting closer. Meanwhile, the goal position is an attractor which attracts the robot to getting close [APF SURVEY]. The force fields are usually based on the distance from the robot to objects (potential field), or the velocity information such as circular field [...].
% Since the robot needs to be aware of the object in the environment, the perception of the object is important, including the position, shape, size of the objects, also the position and motion of the robot (sometimes belong to localization). In previous work, some of the planners are offline, where the perception is pre-processing before the motion planning [...\yuhe{Which one for representative?}]. The online planners, however, have a superior requirement of computational time, which need fast adaptation to re-plan the whole trajectory [..online planner]. From another perspective, some planners need fixed object information, such as the obejct numbers, shapes and so on, and are difficult to adapt to new environments. For example, the neural network needs to be trained every time when adapts into the new environment with different size and number of the objects. Moreover, some planners, especially APF methods, models the object through the spherical approximation, which needs ignores the shape of the objects and needs large parameter tuning about the spherization.
% Concerning these points, we introduce geometric primitives to model the object in the environments. Geometric primitives consist of the points, lines and planes in the 3D environment [...]. For example, a cubic can be viewed as six planes, while the cylinder can be a line with two circular plane on the end points. We calculate the distance between the primitives with the robot, and build the force field based on that. We demonstrate the point-to-line force field, point-to-plane force field, and choose two most common objects - cubic and cylinder to illustrate the superior performance by introducing geometric model. The geometric model can be extended to even more objects such as conic, semi-ellipsoid and so on.
% \textbf{Contributions.} This work introduces a potential field path planning method combining with the geometric model of the environment. Our contributions are threefold: (i) Geometric modeling of the obstacles with relationship between the lines, planes and different 3D objects, (ii) online planning with fast reactive to dynamic scenarios superior in computational complexity, and (iii) motion control considering workspace limit such as singularities. We demonstrate that our methods outperforms the traditional APF methods and circular field methods both in static and dynamic environments. We test both in 3D space tasks and 2D space tasks. The real robot experiments show that the robot can not only tracking the goal for the whole task, but also successfully avoids the obstacles with very fast reactive motion.
\section{GeoPF: System Design and Overview}
\begin{figure}[t]
\centering
\includegraphics[width=1\linewidth]{content/exp/pipeline_all2_compressed.pdf}
\caption{
A snapshot of our GeoPF planner in the same setup as Fig.~\ref{fig:teaser}(\textit{left}). Each obstacle, captured as a sphere, line, plane, cube, or cylinder, generates a distance-based repulsive force. The coloured arrows illustrate the aggregated forces---per geometric type---steering the end-effector (EE) safely to the goal without collision.
% Overview of the proposed motion planner GeoPF.
}
\label{fig::Overview}
\end{figure}
This work is about reactive planning in non-trivial human-centric environments.
% Although straightforward extensions are possible for the whole-body~\cite{becker2025informed}, our current investigation focuses on the end-effector (EE)
$ \dq{x} {\in} \SE{3} $.
Let's consider an initial and a goal pose, respectively $\pose^0, \pose^g {\in} \SE{3}$. Both rotational and translational components are modelled through unit-dual quaternion parametrization, and while the orientation tracking is achieved through screw interpolation \cite{laha2022coordinate},
% \cite{shoemake1985animating,laha2022coordinate},
in this work, we are interested in the Cartesian trajectory, %.
% EE cartesian trajectory critical for collision avoidance.
% This is modelled as a point mass
$\robotposition {\in} \realspace$.
% modeled as point mass $\robotposition \in \realspace$. Let us consider an initial pose and a goal pose $\pose^g$. We take into account both the rotational and translational components of rigid body motion, with the unit dual quaternion parameterization. The full workspace of the robot is denoted by $\workspace \subset \text{Spin}(3) \ltimes \realspace$ and the set of obstacles is indicated by $\workspace_{obs}$, and $\workspace_{\varnothing}$ designates the infeasible workspace containing `inferior' robot configurations such as joint limits, singularities, and self-collisions.
We list all additional notations used herein in Table~\ref{table::notation}.
Furthermore, to improve the mathematical description, we will explore the following functions.
\begin{align}
\normalization(\point) {=} \tfrac{\point}{||\point||}, \quad
\normaldirection(\point_1, \point_2) {=} \normalization(\point^L_{1} {-}\point^L_{2}) = \tfrac{(\point^L_{1} {-} \point^L_{2})}{||(\point^L_{1} {-} \point^L_{2})||}.
\label{eq:distance unitary}
\end{align}
%
where $\normalization(\point)$ and $\normaldirection(\point_1, \point_2)$ are the unit direction along $\point \in \realspace$ and from $\point_2 $ pointing to $\point_1$. %\lf{Is this right? It should be p2 pointing to p1, no?} yes
% The unit direction along a point $\point \in \realspace$ is given by:
% $$\normalization(\point) = \frac{\point}{||\point||},$$ where $||\cdot||$ denotes the Euclidean norm. Given two arbitrary points in Cartesian space $\point_1, \point_2 {\in} \realspace$, we define a function to represent unit direction from $\point_1$ pointing to $\point_2$
% \begin{align}
% \normaldirection(\point_1, \point_2) = \normalization(\point^L_{1}-\point^L_{2}) = \frac{(\point^L_{1}-\point^L_{2})}{||(\point^L_{1}-\point^L_{2})||}.
% \label{eq:distance unitary}
% \end{align}
%Lastly, the notation $<\cdot>$ represents the dot (inner) product, and $\times$ denotes the cross (outer) product.
\begin{table}[t]
\centering
\caption{Notations used throughout the paper}\vspace{-5pt}
\label{table::notation}
\begin{tabular}{cc}
\toprule
Notation & Description \\ \midrule
$<\cdot>$, $\times$ & Dot (inner) and Cross products \\
% $\times$ & Cross (outer) product \\
\midrule
% $<\cdot>$ & Dot (inner) product \\
% $\times$ & Cross (outer) product \\ \midrule
${Geo, S,L,P, B, Y}$ & Geometric Primitives (Geo), Sphere (S), \\
&Line (L), Plane (P), Cubic (B), Cylinder (Y) \\
% ${attr}$ & Attractor \\
% $k$ & Tuning parameter \\
% $\delta_t$ & Step Size in Path Planning \\
% \midrule
% $d$ & The distance \\
$\workspace$ & Robot workspace in $\realspace$ \\
$\workspace^{obs}$ & Geometric Primitive Set \\
$\workspace^{feas}$, $\workspace_{\varnothing}$ & Feasible and Unfeasible Workspace \\
% $\workspace^{feas}$ & Feasible Workspace \\
\midrule
$\acceleration, \velocity, \point$ & Acceleration, velocity, position in Cartesian Space \\
$\point^R$ & Robot Position in Cartesian Space \\
\midrule
% $\pose$ & Pose in Cartesian Space \\
% $\pose^g$ & Goal Pose \\ \midrule
$\ndirection$, $\fdirection$ & Unit and Force Unit direction in Cartesian Space \\
% $\fdirection$ & Force unit direction \\
$\Force$ & Force \\
% $\Force^{\varnothing}$ & Force from robot limitations \\
% $\Force^{res}$ & Total force \\
$r^{Geo}$, $\point_{O}^{Geo}$ & Radius and Center of sphere, circle, cylinder \\
% $\point_{O}^{Geo}$ & Center of sphere, circle, cylinder\\
\midrule
\bottomrule
\end{tabular}
\end{table}
%\luis{LF: We only need to talk about NOKOV in the experimental section. } We employ motion capture devices (XMARS Nokov) to track the environment including the present obstacles. Note that, g
% \noindent\textbf{Environment Representation}
\noindent\textbf{Overview of the Design Strategy}
Given a convoluted obstacle geometric shape, simpler primitives can be fitted to achieve interactive performance. Herein, we make use of---spheres (balls, rounded objects), lines (wires, thin poles), planes (walls, floors), cubes (boxes, crates), and cylinders (pipes, cans)---as these geometric types effectively cover the majority of objects (we elucidate some of them) encountered in real-life scenarios. In the same vein, we designate the space occupied by each type as the spherical space $\workspace^{S}$, the line space $\workspace^{L}$, the plane space $\workspace^{P}$, the cubic space $\workspace^{B}$, and the cylinder space $\workspace^{Y}$. $
\workspace_{obs}$ can now be explicitly defined as $\workspace^{S} \cup \workspace^{L} \cup \workspace^{P} \cup \workspace^{B} \cup \workspace^{Y}$.
%
Thus,
considering the full workspace of the robot $\workspace \subset \SE{3}$,
the feasible space $\workspace^{feas}$ in a non-trivial environment is $\workspace^{feas} = \workspace \setminus (\workspace_{obs} \cup \workspace_{\varnothing})$, where $\workspace_{\varnothing}$ designates the infeasible workspace, e.g., singular-poses, self-collision, joint limits, and workspace out-of-boundaries.
Each geometric primitive is defined by a set of points that describe its spatial configuration in $\SE{3}$. Given the geometric shape and pose information, a geometric model can be constructed for each obstacle in the environment. One key feature to this aim is the normal vector, denoted as $\ndirection^{Geo}$, where $Geo \in \{S, L, P, B, Y\}$ represents the specific geometric type.
For each geometric primitive, exploring $\ndirection^{Geo}$, we are able to project the robot onto the obstacles. The projected point is the perpendicular foot $\point^{Geo}_{\bot R}$. Such a foot system indicates the spatial relationship between them, which in turn defines the avoidance strategy, as depicted in Figs.~\ref{fig::ptl::Field}-\ref{fig::ptY::Force}.
The core principle of potential fields is to model the robot and environment with attractive and repulsive potentials. The attractive force is along the direction from robot pointing to goal position $\fdirection^{attr} = \normaldirection(\point_g, \robotposition).$ The magnitude of the force is controlled by a tuning parameter $k^{attr}$. The attractive force exerted on the robot is given by $\Force^{attr} = k^{attr}\fdirection^{attr}.$ Differently, the repulsive force $\fdirection^{Geo}$ respects geometric considerations and it is directed along the shortest path from the obstacle to the robot. The magnitude of the repulsive force depends on the length of shortest distance $d^{Geo}$ with a tuning parameter $k^{Geo}$:
\begin{align}
\Force^{Geo} = \frac{k^{Geo}}{d^{Geo}}\fdirection^{Geo}.
\end{align}
Therefore, the force is directly proportional to the obstacle distance. The repulsive force should be only active when the robot is close to the obstacle. We set a threshold $\threshold^{Geo}$ for the activation.
$\Force^{\varnothing}$ stems from additional workspace limit-based forces such as self-collision, joint limits, singularities, and workspace limits. The limit-based forces can be expressed through geometric primitives, $\workspace^{\varnothing} \subseteq \workspace^{Geo}$. An example is shown in Fig.~\ref{exp::fig::real}, where boundary limits restrict %the robot movements within
the feasible workspace.
\noindent\textbf{Problem Statement} The robot motion planning problem that we consider is hinged on the notion of potential functions. After the field construction we present earlier, the resultant net force acting on the robot can be expressed as
\begin{align}
\Force^{res} = \Force^{attr} + \sum_{Geo} \Force^{Geo} + \sum_{\varnothing}\Force^{\varnothing},
\end{align}
The robot's acceleration at each timestep can be computed as $\acceleration_i = \Force^{res}_i / \mass$, where $\mass$ denotes the mass of the robot, and $i \in \{1, 2, 3, ...\}$ is the timestep index. The velocity will be updated through $\velocity_{i+1} = \velocity_i + \timeinterval\acceleration_i$, $\timeinterval$ being the step size. Thus, the Cartesian robot position updates as
\begin{align}
\point^R_{i+1} = \point^R_{i} + \frac{1}{2} \timeinterval^2\acceleration_i + \timeinterval \velocity_i.
\end{align}
Lastly, the orientation of the robot is determined by the task requirements, such as the grasping direction. \textit{Thus, we seek a feasible trajectory $\feasiblepath$ starting from $\pose^0$ and ending in $\pose^g$ in the free task space} $\workspace^{feas}$ \textit{brought about by $\Force^{res}$ and taking into account the workspace (environmental) constraints as well as geometrical considerations}.
%\noindent\textbf{Geometric Primitives.} Assuming that obstacles in real-world environments can be represented by various geometric primitives — spheres $S$, lines $L$, planes $P$, cubes $B$, and cylinders $Y$ — these geometric types effectively cover the majority of objects encountered in real-world scenarios. The workspace occupied by each geometric type is denoted as the spherical space $\workspace^{S}$, the line space $\workspace^{L}$, the plane space $\workspace^{P}$, the cubic space $\workspace^{B}$ and the cylinder space $\workspace^{Y}$. The overall obstacle space can be expressed as the intersection of these individual geometric spaces: $\workspace_{obs} = \workspace^{S} \cap \workspace^{L} \cap \workspace^{P} \cap \workspace^{B} \cap \workspace^{Y}$, as shown in Fig.~\ref{TODO}. Therefore, generating the APF for these geometric primitives is essential.
% % \noindent\textbf{Obstacle Repulsion.} Obstacles generate a repulsive force $\fdirection^{Geo}$ that pushes the robot away from them. The direction of this repulsive force starts from the obstacle and points towards the robot. The magnitude of the repulsive force depends on the shortest distance between the robot and the obstacle $d^{Geo}$ with a tuning parameter $k^{Geo}$: $\Force^{Geo} = k^{Geo}d^{Geo}\fdirection^{Geo}$. It is important to note that the normal vector of geometric primitives $\ndirection^{Geo}$ are different with the repulsive force direction $\fdirection^{Geo}$ and their relationship depends on the robot's position. For instance, when the robot is directly above the plane, the repulsive force aligns with the plane's normal vector. However, when the robot moves beyond the edge of a plane segment, the repulsive force follows the line of the shortest distance between the robot and the plane edge.
% \noindent\textbf{Overview of Approach}
% \begin{itemize}
% % \item Each geometric primitive is defined by a set of points that describe its spatial configuration in $\text{Spin}(3) \ltimes \realspace$. One key feature to describe the orientation is the normal vector, denoted as $\ndirection^{Geo}$, where $Geo \in \{S, L, P, B, Y\}$ represents the specific geometric type. Given the geometric shape and pose information, a geometric model can be constructed for each obstacle in the environment.
% % % The unit direction along a point $\point \in \realspace$ is given by: $$\normalization(\point) = \frac{\point}{||\point||},$$ where $||\cdot||$ denotes the Euclidean norm. Given two arbitrary points in Cartesian space $\point_1, \point_2 {\in} \realspace$, we define a function to represent unit direction from $\point_1$ pointing to $\point_2$
% % % \begin{align}
% % % \normaldirection(\point_1, \point_2) = \normalization(\point^L_{1}-\point^L_{2}) = \frac{(\point^L_{1}-\point^L_{2})}{||(\point^L_{1}-\point^L_{2})||}.
% % % \label{eq:distance unitary}
% % % \end{align}
% % \item The resultant force guides the robot's movements at each timestep, generating a trajectory in Cartesian space. For manipulation tasks, the robot's end-effector tracks the Cartesian trajectory and adjusts the arm posture using traditional control strategies, which map the workspace to the configuration space.
% %
% % %
% % \item For each geometric primitive, we project the robot onto the obstacles. The projected point is the perpendicular foot $\point^{Geo}_{\bot R}$. Such a foot system indicates the spatial relationship between the robot and the obstacle,which in turn defines the avoidance strategy, as depicted in Figs.~\ref{fig::ptl::Field}-\ref{fig::ptY::Force}.%\negthickspace%
% % \item The core principle of potential fields is to model the robot and environment with attractive and repulsive potentials. The attractive force is along the direction from robot pointing to goal position $\fdirection^{attr} = \normaldirection(\point_g, \robotposition).$ The magnitude of the force is controlled by a tuning parameter $k^{attr}$. The attractive force exerted on the robot is given by $\Force^{attr} = k^{attr}\fdirection^{attr}.$ Differently, the repulsive force $\fdirection^{Geo}$ we design respects the geometrical considerations. The direction of this repulsive force is along the shortest path from obstacle to robot. The magnitude of the repulsive force depends on the length of shortest distance $d^{Geo}$ with a tuning parameter $k^{Geo}$:
% % \begin{align}
% % \Force^{Geo} = \frac{k^{Geo}}{d^{Geo}}\fdirection^{Geo}.
% % \end{align}
% % Therefore, the force is directly proportional to the obstacle distance. The repulsive force should be only active when the robot is close to the obstacle. We set a threshold $\threshold^{Geo}$ for the activation. %It is important to note that the normal vector of geometric primitives $\ndirection^{Geo}$ are different with the repulsive force direction $\fdirection^{Geo}$ and their relationship depends on the robot's position. For instance, when the robot is directly above the plane, the repulsive force aligns with the plane's normal vector. However, when the robot moves beyond the edge of a plane segment, the repulsive force follows the line of the shortest distance between the robot and the plane edge. Besides, the repulsive force should be only active when the robot is close to the obstacle. We set a threshold $\threshold^{Geo}$ for the activation.
% % $\Force^{\varnothing}$ stems from additional workspace limit-based forces such as self-collision, joint limits, singularities, and workspace limits. The limit-based forces can be expressed through geometric primitives, $\workspace^{\varnothing} \subseteq \workspace^{Geo}$. An example is shown in Fig.~\ref{exp::fig::real}, where boundary limits restrict %the robot movements within
% % the feasible workspace.
% % % $\workspace^{\varnothing} \subseteq \workspace^{Geo}$
% \item Robot motion planning is brought out by the attractive and repulsive forces. The resultant net force acting on the robot can be expressed as
% \begin{align}
% \Force^{res} = \Force^{attr} + \sum_{Geo} \Force^{Geo} + \sum_{\varnothing}\Force^{\varnothing},
% \end{align}
% % \luis{\textbf{LF}: Should we have a numeration like in (5) where I opened the forces (I think we can still have both for emphasis). }\yuhe{yes we can}
% The robot's acceleration at each timestep can be computed as $\acceleration_i = \Force^{res}_i / \mass$, where $\mass$ denotes the mass of the robot, and $i \in \{1, 2, 3, ...\}$ is the timestep index. The velocity will be updated through $\velocity_{i+1} = \velocity_i + \timeinterval\acceleration_i$, $\timeinterval$ being the step size. Thus, the Cartesian robot position updates as
% \begin{align}
% \point^R_{i+1} = \point^R_{i} + \frac{1}{2} \timeinterval^2\acceleration_i + \timeinterval \velocity_i.
% \end{align}
% Lastly, the orientation of the robot is determined by the task requirements, such as the grasping direction.
% % \item{Motion Control.} With the given positions and orientation of in Cartesian space, the robot can demonstrate tasks in Cartesian space with avoiding the obstacles and workspace limits. In this work, we introduce the inverse kinematic (IK) controller to adapt the whole body posture adaptation of the robot arm, based on the desired End-effector position and orientation from GeoAPF. To guarantee the smooth movements of the robot, we choose the solution that closest to the actual joint angles at each time step.
% \end{itemize}
% \noindent\textbf{Cubic Direction.}
% Circular Plane: suppose we know the circle center $\point^{Pc}_0$ and one point on the edge of the circle $\point^{Pc}_1$. Besides, the normal vector of circular plane is known to be align with the cylinder axis. Then it is easy to find a
\section{From Geometry to Force Generation}\label{sec:geometry_to_force}
One of the key contributions of this work is a geometric-based environment representation that explicitly encodes the spatial properties of objects and uses this geometry to drive temporal motion generation. Rather than relying solely on point clouds or large sets of spheres for obstacle modeling, \revised{or the closest-point contact-based to the obstacle,} we cast primitives—including spheres, lines, planes, cubes, and cylinders—into repulsive forces within a vector field.
% In this work, we focus on five geometric primitives: sphere, line, plane, cube, and cylinder. We introduce two key features for each geometric type: the points that define the position of the geometric primitives and the normal vector that determines their orientation
% \lf{ I didn't understand the idea behind the normal vector determines their orientation.} \yuhe{The normal vector means the unit vector along line direction, the unit vector orthogonal to plane and so on. So it determines the orientation I guess. When the direction of normal vector changes that means the line changes right? }\lf{Yeap, but the doesn't define the orientation per si, right? The orientation will be defined by a rotation along this normal vector axis (Assumin for instance itis defined a z-axis, then x and y will be undefined until we define the rotation along the axis. Maybe we remove the idea of the orientation, or rephrase it? :) } \yuhe{oh yes, i just want to express that the normal pointing out the direction the gps face to. Not the rotation at all. }.
%let $\robotpoint \in \realspace$ be the position of the robot and
Formally, let $\{G_{i}\}$ denote the set of geometric primitives representing the obstacles. Each primitive $G_{i}$ is associated with its own distance function $d_{i}(\cdot)$ and corresponding repulsive force $\Force_{i}$. The aggregate motion is determined by summing these repulsive forces with an attractive force pulling the robot toward the goal $\point^{g}$. Specifically,
\begin{equation}
\Force^{\mathrm{res}} =
\underbrace{k^{attr}\fdirection^{attr}}_{\text{goal attraction}}
+
\sum_{i}
\negthickspace \negthickspace \negthickspace \negthickspace
\underbrace{
\left(\frac{k_i}{\,d_i(\cdot)\,} \fdirection_i\right)}_{\text{repulsion from obstacle }i}
\negthickspace \negthickspace \negthickspace
+ \
\sum_{j} \Force^{\varnothing}_j,
\label{eq:f_res}
\end{equation}
%
%
where $k_{\mathrm{attr}}$ and $k_i$ are scalar gains.
%
%$\fdirection^{attr} = \normaldirection(\point_g, \robotposition)$
% \[
% \mathbf{f}_{\mathrm{attr}} \;=\; \frac{\bigl(\mathbf{p}_{g}-\mathbf{p}_{R}\bigr)}{\|\mathbf{p}_{g}-\mathbf{p}_{R}\|}
% \]
%is the unit attractive direction, and
%$\Force^{\varnothing}_j$ stems from the additional limit forces such as self-collision, joint limits, singularities, and workspace limits.
The following subsections detail how each geometric primitive defines its distance function $d_{i}(\cdot)$ and force direction~$\mathbf{f}_i$.
\subsection{\textbf{Point and Sphere-based Potential Field}}
A sphere is defined by its centroid $\spherecenter {\in} \realspace$ and radius $\sphereradius$, while a point is given by $\sphereradius {=} 0$. It forms the simplest geometric primitive we define for collision avoidance. First, we compute the \textit{shortest distance} ($\distancesphere$) between the point robot and the sphere’s surface, i.e., $d^S {=} || \robotpoint - \spherecenter|| - \sphereradius$.
%
Second, we are interested in the direction of the relative position vector, i.e.,
the radial vector outward from the sphere center, $\fdirection^{S} {=} \normaldirection(\point^R, \spherecenter) $.
%
% $\fdirection^S = D(\point^R, \spherecenter)
% = \frac{ \point^R {-} \spherecenter }{ \norm{ \point^R {-} \spherecenter }}$.
% %
The repulsive force is therefore computed as
\begin{equation}
\Force^{S} = \tfrac{k^{S}}{d^{S}} \fdirection^{S} ,
\quad
\text{where }
%
\fdirection^{S} {=} \tfrac{ \point^R {-} \spherecenter }{ || \point^R {-} \spherecenter || },
\ \
k^{S} {>} 0.
\end{equation}
%
The sphere-based repulsive force can be enhanced by circular forces or magnetic-based \cite{ataka2022magnetic,laha2023predictive}, yet we explicitly decided to maintain a straightforward approach, avoiding unnecessary complexity of the spherization---that often rely on tuning for efficiency, as shown in Section \ref{sec:QuantitativeAnalysis}. For more details on alternative forces, please refer to \cite{laha2023predictive,becker2023motion}.
% $\Force^{S} = \frac{k^{S}}{d^{S}} \fdirection^{S} $, where $k^{S} {>} 0 $ is the repulsive gain
% The repulsive force direction is the radial vector outward from the sphere center,
% eq:distance unitary
%\luis{LF.: PLease check above if all the information about the sphere is present. I removed the computation of the normal vector and point in the surface (I don't think we were using it right?)}
%We define the sphere by a centroid point $\spherecenter \in \realspace$ and radius $\sphereradius$. Given an arbitrary point on the surface of the sphere, denoted as $\point^S_1 \in \realspace$, the normal vector at that point can be expressed as $\ndirection^S = \normaldirection(\point^S_1,\spherecenter)$. The sphere force is pointing from the sphere center to the robot $\fdirection^S = D(\point^R, \spherecenter)$. The magnitude of the force is defined by the reciprocal distance between robot and sphere surface $d^S = ||\point^R - \spherecenter|| - \sphereradius.$ The sphere force is $\Force^{S} = \frac{k^{S}}{d^{S}}\fdirection^{S}$.
% ######################
% ######################
% ###################### Plane Field
% ######################
% ######################
\subsection{\textbf{Segment Line-based Potential Field}}
\label{sec::linefield}
In the remainder of this section, we present additional geometric-primitive-based potential field generation. Starting from a line segment, defined
% A line segment, defined
by two vertices % ex points
$\point^L_{1}, \point^L_{2} {\in} \realspace$.
% \luis{\textbf{LF: Are we using these variables}?!}
% \luis{
% , can be denoted as $L_{\point^L_{2}\to \point^L_{1}}$ (abbr. $L^L_{2\to1}$) that pointing from $\point^L_{1}$ to $\point^L_{2}$ . The direction of the line segment $\ndirection^L$ can be expressed as $\ndirection^L_{2\to1} = \normaldirection(\point^L_{1}, \point^L_{2})$. The length of the line is $l^L = ||\point^R_2 - \point^R_1||$.
% }
% \noindent\textbf{Line Force. }
% \luis{LF: $=>$} directed along \luis{$<=$ ?}
The repulsive force surrounding the line segment is a function of the shortest path from the line to the robot. The shortest distance, and thereafter, the repulsive force direction, between the robot ($\robotpoint$) and the finite segment may arise from one of two cases, as shown in Fig.~\ref{fig::ptl::Field}.
\begin{enumerate}
\item The robot projected onto the line segment, as shown in Fig~\ref{fig::ptl::Field1}, indicating that the perpendicular foot lies between the vertices. In the other words, the robot is orthogonal to the line segment. %The \yuhe{Here, the notation $\workspace^L_o$ means the space occupied by the line and robot cannot go into. I am also confused how to describe, so finally I write - if the foot projected from robot to the line is inbetween $\workspace^L_o$.}\lf{Do you think we can keep the description:}\yuhe{It is okay for me to delete}\lf{But, then we need to rewrite. We are saying 2 cases, so I used the enumerate.} Orthogonal region: If the robot lies in the orthogonal area inbetween end-points $\workspace^L_o$, that is, if the perpendicular foot of $\robotpoint$ onto the infinite line lies between endpoints;
\item Otherwise, the robot lies outside the orthogonal region, as shown in Fig~\ref{fig::ptl::Field2}. In the other words, the robot is on the side of the line segment.
\end{enumerate}
In the first case, the resulting orthogonal-based force direction is defined as $\fdirection^L_o$, whereas the side forces are given by $\fdirection^L_{s1}$ and $\fdirection^L_{s2}$, as shown in Fig.~\ref{fig::ptl::Field}.
To evaluate both conditions,
we first compute the perpendicular foot that the robot projects onto the line.
%%%%%%
To compute it, we first
obtain the robot-line plane based on the line within the plane, e.g.,
%
$\ndirection^L_{1\to R} {=} \normaldirection(\point^R,\point^L_1)$,
%
and the plane's normal direction, given by
$\ndirection^{RL} {=} %\normalization
(\ndirection^L_{1\to R} {\times} \ndirection^L)$, where
%
$\ndirection^L {=} \ndirection^L_{2\to1} {=} \normaldirection(\point^L_{1}, \point^L_{2})$ is the unitary vector from $\point^L_{2}$ towards $ \point^L_{1}$. Next, we compute the orthogonal direction perpendicular to both the line and the plane normal, i.e.,
$\ndirection^L_o = % \normalization
(\ndirection^L {\times} \ndirection^L_R)$.
%
Finally, the orthogonal distance from the robot to the line is given by
$d^L_{o} = <\point^R {-} \point^L_1, \ndirection^L_o>$, which is the length of the projection of the robot to $\point^L_1$ over $\ndirection^L_o$.
%
The perpendicular foot, in this case, also the projection point, can be calculated as $\point_{\bot R}^L = ( \point^R {-} d^L_{o} ) \ndirection^L_o$.
When the perpendicular foot $(\point_{\bot R}^L)$ lies between the vertices of the line segment, i.e., $\point_{\bot R}^L {\in} \workspace^L$,
% \lf{\textbf{ Before we were using $\workspace^L_o$ for the area! We need to be careful with the notation. It is confusing to use the same symbol for area and for the segmented line (we also didn't define it). There are many ones we are not using as well (which overcomplicates the reading)}}
the repulsive force is the orthogonal force.
% \lf{\textbf{ Do we need this? We are adding new notation (Also not defined) and it is saying the same thing as the item: $\point_{\bot R}^L {\in} \workspace^L$. I would remove this reference and use the case-scenario (Eq. 5) with this, avoiding introducing the new distance and line distance.}}
In this case, the distance from the perpendicular foot to the edge is always smaller than the line distance, i.e., $d^L_1 < l^L \cap d^L_2 > l^L$. Note that the distance can be calculated using the Euclidean norm, given the known position of perpendicular foot and the vertices.
Conversely, when the perpendicular foot is outside the line segment $\point_{\bot R}^L\not\in\workspace^L$, the repulsive force is the side force, directed from the closest vertices towards the robot:
\begin{subequations}
\begin{empheq}[left={(\fdirection^L, d^L) = \empheqlbrace}]{align}
& (\fdirection^L_{s1},d^{L}_{1}) & \text{$d^L_1 > l^L \cap d^L_2 > l^L$}, \label{lineforceA}\\
& (\fdirection^L_{o}, d^L_o) & \text{$d^L_1 < l^L \cap d^L_2 < l^L$}, \label{lineforceB}\\
& (\fdirection^L_{s1},d^L_1) & \text{$d^L_1 < l^L \cap d^L_2 < l^L$}, \label{lineforceC}
\end{empheq}
\begin{align}
\Force^{L} = \tfrac{k^{L}}{d^{L}} \fdirection^{L}.
\label{lineforceD}
\end{align}
\end{subequations}
% \lf{\textbf{ I think we can replace this with the text i wrote before, but please double check.}}
% \lf{ There are two possible directions for the force: one is orthogonal to the line, denoted as $\fdirection^L_o$, while the other lies outside the orthogonal region and points from the vertex, denoted as $\fdirection^L_{s1}$ and $\fdirection^L_{s2}$, as shown in Fig.~\ref{fig::ptl::Field}. We define the orthogonal area beside the line as $\workspace^L_o$. Given the robot's position $\point^R$ and the line geometry defined by $\point_{1,2}^L$, the subsequent parts will calculate these two types of forces. }
\begin{figure}[t]
\centering
\begin{subfigure}{0.49\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/field_pic/ptl_field.pdf}
\caption{Orthogonal Line Field}
\label{fig::ptl::Field1}
\end{subfigure}
\begin{subfigure}{0.49\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/field_pic/ptl_field2.pdf}
\caption{Side Line Field}
\label{fig::ptl::Field2}
\end{subfigure}
\caption{%
Line-based potential field. (a) When the perpendicular foot is inside the line segment. The force is orthogonal. (b) When the perpendicular foot is outside the segment, the force is side force. }% (a) Within the shaded orthogonal region, the shortest path \lf{what is? I think the description in the caption is wrong, no? Or my comment is wrong?}\lf{(Yuhe: Isn't it that the plane normal is orthogonal? the force is orthogonal to plane normal and $\ndirection^L$)} is perpendicular to the line $\fdirection^L_o \bot \ndirection^L$. The blue area is the plane (end-points to the robot) orthogonal to $\fdirection^L_o \bot \ndirection^{RL}$ (Eq.~\ref{lineforceB}). (b) Outside this region, the force ($\fdirection^L_{s1}$) originates from the nearest endpoint. \lf{Yuhe: Update the figure. }
%
% The gray area is the orthogonal region. (a) Inside this region, the shortest path is orthogonal to the line direction $\fdirection^L_o \bot \ndirection^L$. The blue area is the plane shaped by robot and line. The plane normal is also orthogonal to force direction $\fdirection^L_o \bot \ndirection^{RL}$ (Eq.~\ref{lineforceB}). (b) Outside this region, the shortest path points from the closest vertex to the robot. Here we demonstrate $\fdirection^L_{s1}$ (Eq.~\ref{lineforceA}). \yuhe{originally, i was thing to use $\workspace^L$ to denote the line that robot cannot go into. and $\workspace^L_o$ to denote the orthogonal area beside the line that robot can go into. but seems it causes confusion. Any good idea? }\lf{I think we can have the whole gray-shaded area as a region. If the robot lies inside this area, then we have one type of repulsive force, otherwise, if we have the point distance. ps.: Saying a area that the robot cannot go in seems too restrictive (it is a repulsive force, but the robot can still get closer to the line (inside the shaded area), up to the point where the line force exceeds the attractor.}
% \lf{ Can you also add the $\workspace^L or \workspace^L_o$ in the figure (it is useful to understand that this is the gray area). :) }
%}
\label{fig::ptl::Field}
\end{figure}
% \yuhe{here, i want to explain how to calculate the perpendicular foot. Should we keep it or leave it out? } \lf{The perpendicular foot is given by: $\point_{\bot R}^L = ( \point^R {-} d^L_{o} ) \ndirection^L_o$, no? } \yuhe{oh yes. }
% \lf{\textbf{I think we can replace this with the text i wrote before, but please double check.}}
% \lf{
% The distances from robot to vertex points are $d^L_1 = ||\point_{1}^L - \point^R||$ and $d^L_2 = ||\point_{2}^L - \point^R||$. Either to choose the orthogonal force or the sides forces, it depends on the \textit{perpendicular foot} that the robot projects onto the line. The process of obtaining the perpendicular foot consists of three steps. First, we determine the second line in robot-line plane as $\ndirection^L_{1\to R} = \normaldirection(\point^R,\point^L_1)$. The plane's normal direction is $\ndirection^{RL}= \normalization(\ndirection^L_{1\to R}\times\ndirection^L)$. Next, the orthogonal direction, which is perpendicular to both the line and the plane normal, is given by $\ndirection^L_o = \normalization(\ndirection^L\times\ndirection^L_R)$. Finally, the orthogonal distance from the robot to the line is $d^L_{o} = <\point^R-\point^L_1, \ndirection^L_o>$. And the perpendicular foot can be calculated as $\point_{\bot R}^L = \point^R - d^L_{o} \ndirection^L_o$. As a result, when the perpendicular foot lies between the vertex points of the line segment $\point_{\bot R}^L\in\workspace^L$, the repulsive force is the orthogonal force. In this case, the distance from the perpendicular foot to the edge is always smaller than the line distance, i.e., $d^L_1 < l^L \cap d^L_2 > l^L$. Conversely, when the perpendicular foot is outside the line segment $\point_{\bot R}^L\not\in\workspace^L$, the repulsive force is the side force, directed from the closest vertex towards the robot:
% }
%
%
%
%Therefore, The line force is $\Force^{L} = \frac{k^{L}}{d^{L}}\fdirection^{L}$.
%\lf{Add the forces $\Force^{L}$ in the equation 5, so we can cite it directly. }
% ######################
% ###################### Plane Field
% ######################
\subsection{\textbf{Plane-based Potential Field}}
\label{sec::planefield}
Planes are among the most common geometric obstacles in human-centred environments---representing tables, walls, and other flat surfaces. Traditional methods approximate them using multiple discrete spheres, as shown in Fig.~\ref{fig:exp:plane scenario1}. Although conceptually simple, this results in inefficient and imprecise representations, requiring extensive tuning to prevent the field from either penetrating the spheres or generating unnecessarily large repulsive forces. In contrast, explicitly considering a plane with four vertices allows for closed-form distance and force computations, reducing both computational overhead and tuning complexity.
%
Indeed, let's consider a plane defined by four (counter-)clockwise ordered vertices $\point^{P}_{1},\point^{P}_2,\point^{P}_3,\point^{P}_4 {\in} \realspace$.
%
The plane's outward normal can be computed as
% \lf{Do we need to define $L_{2\to1}^P: L_{\point^P_{2}\to \point^P_{1}}$? This seems an additional notation that we are not using. It was also in the line distance. Take a look if the paragraph above and below eq. 7 is sufficient. :) } \yuhe{yes I think we don't need them. }
% Since planes in the real world are often used to represent objects such as tables, walls, and other rectangular surfaces, we define a plane by four vertex points, denoted as $\point^{P}_{1,2,3,4}\in \realspace$, ordered clockwise or counterclockwise. The normal vector of a plane is defined as the vector orthogonal to the plane surface. By choosing a vertex point $\point^P_{2}$, two adjacent edge lines can be abbreviated as $L_{2\to1}^P: L_{\point^P_{2}\to \point^P_{1}}$ and $L_{2\to3}^P: L_{\point^P_{2}\to \point^P_{3}}$. Subsequently, the normal directions of these edges are $\ndirection_{2\to1}^P=\normaldirection(\point^P_{1}, \point^P_{2})$ and $\ndirection_{2\to3}^P=\normaldirection(\point^L_{3}, \point^L_{2})$, respectively. The direction of the plane can be calculated by cross product
% %
% \lf{We don't need the normalization for unitary x unitary operations.}\yuhe{even though they are not orthogonal? \lf{Ahh yeap, you are right (although, we have a rectangular plane, right?), so p2_1 and p2_3 should be orthogonal. :) But, I don't think we need to normalize the direction between p2 to p1, and p3 to p2, since the magnitude of the segment line will only affect the magnitude of the cross-product, and not the direction. } }
\begin{align}
\ndirection^{{P}} = \normalization(\ndirection_{2\to1}^P \times \ndirection_{2\to3}^P),
\label{eq::planedirection}
\end{align}
%
where
$\ndirection_{2\to1}^P=\normaldirection(\point^P_{1}, \point^P_{2})$ and $\ndirection_{2\to3}^P=\normaldirection(\point^L_{3}, \point^L_{2})$ depict segments of line within the plane.
%
% \noindent\textbf{Plane Force.}
Similar to the segment-line case,
the repulsive force, thus the avoidance strategy, is determined by the robot's projection onto the perpendicular foot ($\point_{\bot R}^P$) on the obstacle plane.
%
The perpendicular foot on the plane is computed by
$\point^P_{\bot R} = \point^R {-} d^P_o\ndirection^P$
where
$d^P_o = {<{\ndirection^P, \point^R {-} \point^P_1}>}$
is the spatial distance between the plane and the robot projected on the orthogonal outward normal from \eqref{eq::planedirection}.
% \lf{Yuhe: The notation here is $\ndirection^P_o$ and $\ndirection^P$ (from (7), I assume they are the same, or not?} \yuhe{yes. they are same, i changed them all to $\ndirection^P$}
%
% \lf{Yuhe, please check if we can delete the following now.}\lf{
% the plane field also considers the orthogonal force $\Force^P_o$ and the side force $\Force^P_s$. The robot is projected onto the perpendicular foot $\point_{\bot R}^P$ on the plane. The plane is defined by four vertex points $\point_{1,2,3,4}^P$, where the edges are the lines connecting these points. When the perpendicular foot lies outside the plane segment, i.e., $\Force^P_o \not\in \workspace^P$, the force originates from the closest edge and points towards the robot, which corresponds to the line force as described in Sec.~\ref{sec::linefield}. In contrast, when the perpendicular foot lies within the plane segment, i.e., $\Force^P_o \in \workspace^P$, the plane force aligns with the plane normal.}
%
\begin{figure}[t]
\centering
\begin{subfigure}{0.49\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/field_pic/ptp_ortho.pdf}
\caption{Orthogonal Plane Field}
\label{fig::ptp::Field1}
\end{subfigure}
\begin{subfigure}{0.49\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/field_pic/ptp_side.pdf}
\caption{Side Plane Field}
\label{fig::ptp::Field2}
\end{subfigure}
\caption{Plane-based potential field defined by the perpendicular foot $\point^P_{\bot R}$. (a) If it lies within the plane boundary, the condition vectors (green arrows) remain aligned, see \eqref{eq::pointinsideplane}, then the repulsive force direction $\fdirection^P$ aligns with the plane’s normal. (b) Otherwise, the force is derived from the nearest edge.}
%
%\lf{\textbf{TO REMOVE: }}\lf{
%If the foot lies within the plane, as illustrated in (a), The angle condition vectors remain aligned in the same direction (green arrows), as a result of the cross product between the foot-vertex line (dotted gray lines). However, when the foot is outside the plane, as shown in (b), the angle from $\ndirection^P_{r 2}$ to $\ndirection^P_{r 3}$ exceeds 180° according to the right-hand rule, which causes the reverse direction of $\boldsymbol{\underline{c}}_2$.}
%}
\label{fig::ptp::Field}
\end{figure}
% \lf{Yuhe, confirm if we can remove the following: }\lf{
% The distance from the robot to the plane is the projected distance $d^P_o = <\ndirection^P_o, \point^R - \point^P_1>$. Therefore, the perpendicular foot on the plane is $\point^P_{\bot R} = \point^R - d^P_o\ndirection^P_o$, where $\ndirection^P_o$ is calculated according to Eq.~(\ref{eq::planedirection}).
% }
% \lf{Yuhe, confirm if we can remove the following:}\lf{
% And orthogonal force direction is $\fdirection_o^P = \ndirection^P_o$. The side force is defined as the force acting from the nearest edge of the plane to the robot. The nearest edge can be determined by calculating the distances between the robot and the vertex points. The two nearest vertex points shape the nearest edge. Then the side force is calculated according to Sec.~\ref{sec::linefield}.
% }
% The criterion for determining
Whether the perpendicular foot lies within a rectangle's plane can be evaluated by
%analyzing the
angles between the lines connecting the foot point and the rectangle's vertices. Let the direction from the perpendicular foot to the $i^{th}$ vertex be denoted as $\ndirection^P_{r i}$, where $i\in\{1,2,3,4\}$. If the perpendicular foot is located within the plane, the angles between two adjacent foot-vertex lines should be less than 180°.
%
In contrast, if any of the angles exceed 180°, the point lies outside the plane.
The angle condition can be determined through the cross-product
$< \negthinspace \ndirection^P_{r i}, \ndirection^P_{r( i+1)} \negmedspace >$.
Therefore, we have the normal direction of the plane given by
\begin{align}
\boldsymbol{\underline{c}}_1 :=
\normalization(\ndirection^P_{r 1} \times \ndirection^P_{r 2}),
\quad
\boldsymbol{\underline{c}}_2 :=
\normalization(\ndirection^P_{r 2}\times\ndirection^P_{r 3}), \\
\boldsymbol{\underline{c}}_3 :=
\normalization(\ndirection^P_{r 3}\times \ndirection^P_{r 4}),
\quad
\boldsymbol{\underline{c}}_4 := \normalization(\ndirection^P_{r 4}\times \ndirection^P_{r 1}).
\label{eq::pointinsideplane}
\end{align}
% \lf{Yuhe, here $\myvecx{c}_i$ with $i{=}$\{1..4\} don't need to be equal, right? Just the sign between them?} \yuhe{I think we address this issues}.
The perpendicular foot lies inside the plane if and only if all
% When all the angle
indicators $\boldsymbol{\underline{c}}_i$ point in the same direction, i.e.,
$\boldsymbol{\underline{c}}_1 {=}
\boldsymbol{\underline{c}}_2 {=}
\boldsymbol{\underline{c}}_3 {=}
\boldsymbol{\underline{c}}_4$, %as seen in
Fig.~\ref{fig::ptp::Field1}. %, the perpendicular foot lies inside the plane.
%
In this case, the perpendicular foot lies inside the plane boundaries $\point^P_{\bot R} \in \workspace^P$, and the repulsive force direction is orthogonal to the plane and aligned to \eqref{eq::planedirection}, i.e., $\fdirection_o^P = \ndirection^P = \boldsymbol{\underline{c}}_i$.
% orthogonal force $\Force^P_o$ and the side force $\Force^P_s$.
% $\Force^P_o \in \workspace^P$, the plane force aligns with the plane normal.
% When the perpendicular foot lies outside the plane segment, i.e., $\Force^P_o \not\in \workspace^P$, the force originates from the closest edge and points towards the robot, which corresponds to the line force as described in Sec.~\ref{sec::linefield}.
When any of $\boldsymbol{\underline{c}}_i$ points in the opposite direction (Fig.~\ref{fig::ptp::Field2}), the point is outside the plane, $\point^P_{\bot R} \not \in \workspace^P$, which guarantees the convexity of the plane. The resulting side force $\fdirection_s^P$ and corresponding distance $d_s^P$ is then redirected from the nearest plane edge or vertex using line-based logic as in Sec.~\ref{sec::linefield}.
The resulting plane repulsion is computed as
\begin{equation}
\Force^P = \tfrac{k^P}{\,d^P} \fdirection^P,
\label{eq:plane:force}
\end{equation}
where
$(\fdirection^P, d^P)$ is given by either $(\fdirection_o^P,d_o^P)$ or $(\fdirection_s^P,d_s^P)$ according to the case described above.
\noindent\textbf{Plane-based Correction Potential Field Heuristic}
While successful in avoiding collision with plane-based geometries,
% However,
when a large plane is positioned between the robot and the goal, %one issue is that
the attractive force counteracts the repulsive force, causing a trap condition (Fig.~\ref{fig::ptp::Correction1}).
% the robot to continuously attempt to cross the plane. Therefore, a force correction is required to address this issue.
If the line from robot to goal intersects the plane interior, we project a repulsive force parallel to the plane’s surface (toward the nearest plane edge) rather than purely along the normal. This prevents the attractor from canceling out the plane’s repulsion, allowing the robot to “slide” around or above the plane.
% At first, we should know if the robot-goal line intersects with the plane.
Given the unit vector along the robot-goal line, $\ndirection^{RG} = \normalization(\point^R-\point^G)$,
the intersection point in the unbounded plane is given by
% Since we know the plane normal direction, we can get the intersection point through
$\point^P_{inter} {=} \point^R {-} d^P_{inter}\ndirection^{RG}$,
where $d^P_{inter} {:=} \frac{d^P_o}{<\ndirection^{RG}, \ndirection^P_o>}$ is the distance from the robot to the intersection point.
% \begin{align}
% d^P_{inter} = \frac{d^P_o}{<\ndirection^{RG}, \ndirection^P_o>}.
% \end{align}
If the intersection point lies within the plane, we project a force parallel to the plane toward the nearest plane edge.
% we adjust the force to be parallel to the plane, ensuring it moves over the plane without crossing it.
By defining the nearest edge direction $\ndirection^P_{e}$,
% the robot should always go towards the nearest edge to over the plane. The corrected
the parallel direction is given by $\ndirection^P_{corr}=\ndirection^P\times \ndirection^P_{e}$. %\lf{Yuhe, (The notation $\ndirection^P$)}
To avoid crossing
% However, the cross product may point
toward the farthest edge, see Fig.~\ref{fig::ptp::Correction2},
% To resolve this issue, we introduce the
we evaluate the direction from the robot to the nearest edge{\footnote{\revised{In rare case of exact symmetry, similarly to \cite{laha2023predictive}, a small random additive noise is applied toward one of
the primitive’s vertices or edges.}}},
denoted as $\ndirection^{PR}_{e}$, which corresponds to the opposite direction of the line-to-robot vector, Section~\ref{sec::linefield}.
% We further define a sign function as
% \begin{empheq}[left={sign(h) = \empheqlbrace}]{align}
% & 1 & h > 0 \in \mathbb{R}, \tag{8a} \\
% & -1 & h < 0 \in \mathbb{R}. \tag{8b}
% \end{empheq}
By computing $s = \text{sign}(<\ndirection^P_{corr}, \ndirection^{PR}_{e}>)$, we can determine whether the parallel direction toward the nearest edge or its opposite. When $s=1$, the direction is correctly toward the nearest edge ($\alpha < \frac{\pi}{2}$), whereas $s=-1$ indicates the farthest edge ($\alpha > \frac{\pi}{2}$), see Fig.~\ref{fig::ptp::Correction}.
Accordingly, the corrected force is % defined as $\fdirection^P_{corr} = s\ndirection^P_{corr}$.
\begin{equation}
\fdirection^P_{corr} = s\ndirection^P_{corr},
\quad
\text{with} \ \
\ndirection^P_{corr}{=}\ndirection^P {\times} \ndirection^P_{e},
\label{eq:plane:correction force}
\end{equation}
% \yuhe{
with the magnitude of the force according to the distance between the robot and the plane - the orthogonal force and the side force corresponding distance.
% }
\tofullversion{
\lf{I like the description (but perhaps we leave it for the next journal. We are running out of space). Let's save a copy with all these expressions, but for the submitted version we cut it due to space:}
Summarized, the plane force can be expressed as
\begin{empheq}[left={(\fdirection^P, d^P) = \empheqlbrace}]{align}
& (\fdirection^P_{o},d^{P}_{o}) & \text{$\point^P_{\bot R},\in \workspace^T, \point^P_{inter}\notin \workspace^P$}, \tag{5a}\label{PlaneforceA}\\
& (\fdirection^P_{s},d^{P}_{s}) & \text{$\point^P_{\bot R},\notin \workspace^T, \point^P_{inter}\notin \workspace^P$}, \tag{5b}\label{PlaneforceB}\\
& (\fdirection^P_{corr},d^{P}_{o}) & \text{$\point^P_{\bot R}\in \workspace^P, \point^P_{inter}\in \workspace^P$}, \tag{5c}\label{PlaneforceC} \\
& (\fdirection^P_{corr},d^{P}_{s}) & \text{$\point^P_{\bot R}\notin \workspace^P, \point^P_{inter}\in \workspace^P$}, \tag{5d}\label{PlaneforceD}
\end{empheq}
\begin{align}
\Force^{P} = \frac{k^{P}}{d^{P}} \fdirection^{P}. \tag{5e}
\label{PlaneforceE}
\end{align}
}
% \lf{Yuhe: Should we mention the magnitude of the force?!} \yuhe{yes we need. because we have a magnitude of the reciprocal of distance, so we don't need too much parameter tuning later in experiment - we use all $k^{Geo}=0.1$. because the force is $\Force=\frac{k}{d}\fdirection$. But the magnitude is always the distance. so it is already calculated in previous subsections - we only correct the direction. not about the magnitude. the magnitude is according to the orthogonal or the side situation.}
\begin{figure}[t]
\centering
\begin{subfigure}{0.49\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/field_pic/ptp_correction1.pdf}
\caption{Fail to avoid the plane}
\label{fig::ptp::Correction1}
\end{subfigure}
\begin{subfigure}{0.49\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/field_pic/ptp_correction2.pdf}
\caption{Plane Field Correction}
\label{fig::ptp::Correction2}
\end{subfigure}
\caption{Plane force correction. (a)
Without correction, a large plane may trap the robot by canceling the repulsive and attractive forces.
% As the repulsive force eliminated by the attractive force, the robot is unable to go over the plane and instead collides with it.
(b)
When the line connecting the robot and goal intersects with the plane ($\point_{inter}^P$), the force is redirected to the parallel direction
$\ndirection^P_{corr}$. If the parallel direction points aways of the nearest edge, the force is redirected according to $\alpha$ with $\fdirection^P_{corr}$ being the corrected direction.}
\label{fig::ptp::Correction}\vspace{-10pt}
\end{figure}
% [0, −1, 0] -of-magnitude improvements in computational speed over
% state-of-the-art reactive field-based planners while maintaining
% higher success rates and superior trajectory quality.orders-of-magnitude improvements in computational speed over
% state-of-the-art reactive field-based planners while maintaining
% higher success rates and superior trajectory quality.
\textcolor{blue}{GeoPF has} orders-of-magnitude improvements in computational speed over
state-of-the-art reactive field-based planners while maintaining
higher success rates.
% . Plane force correction. (a) Without correction, a large plane may trap the robot by canceling the repulsive and attractive forces. (b) Redirecting the force parallel to the plane lets the robot “slide” around the obstacle
% ######################
% ######################
% ###################### Cubic Field
% ######################
% ######################
\subsection{\textbf{Geometric Cube-Based Potential Field}}
Extending from previous primitives,
many everyday objects, from boxes to shelves and cabinets, can be approximated as cubes or rectangular prisms. To model a cube, we use eight (counter-)clockwise ordered vertices $\point^{B}_{i} {\in} \realspace$, \ i{=}\{1,...,8\}.
% Similar to plane, the cubic are shaped by eight vertex points
% $\point^{B}_{1,2,3,4,5,6,7,8}\in \realspace$. The vertices are ordered consistently in either a clockwise or counterclockwise direction, with
The vertices $\point^{B}_{1,2,3,4}$ represent one surface, whereas $\point^{B}_{5,6,7,8}$ representing the opposite surface. Moreover, $\point^{B}_{1}$ is aligned with $\point^{B}_{5}$, as illustrated in Fig.~\ref{fig::ptb::Force}. Therefore, the cube can be decoupled into six different surfaces $\workspace^B_{S1}$ to $\workspace^B_{S6}$ and their intersection edge $\workspace^B_{Si} \cap \workspace^B_{Sj}$, with $i\neq j \in \{1,2,3,4,5,6\}$.
% \noindent\textbf{Cubic Force.}
% The repulsive force is determined based on the nearest surface. At first, the distances to all surfaces are computed and compared. The closest plane is then selected to generate the plane force following the methodology outlined in Sec.~\ref{sec::planefield}, as shown in Fig.~\ref{fig::ptb1}.
In practice, each of the six faces is treated like an individual plane (as in Section~\ref{sec::planefield}), so the force generation relies on choosing whichever face is nearest, as shown in Fig.~\ref{fig::ptb1}.
When the robot is closest to a cube edge $\workspace^B_{Si} {\cap} \workspace^B_{Sj}$, then the forces generated by $\workspace^B_{Si,j}$ are equivalent and the distance and direction of repulsion are handled via plane or line logic, whichever is more appropriate according to Section~\ref{sec::linefield}. In summary,
\begin{align}
\Force^{B} = \frac{k^{B}}{d^{B}} \fdirection^{B}, (\fdirection^B, d^B) = (\fdirection^B_{Si},d^{B}_{Si}), i=\arg\min^6_{i=1} d^B_{Si}.%\tag{5e}
\label{Cubicforce}
\end{align}
% . Either surface can be selected to generate the forces according to Sec.~\ref{sec::planefield}. %, as shown in Fig.~\ref{fig::ptb2}.
% In practice, each of the six faces is treated like an individual plane (as in Section ~\ref{sec::planefield}), so the force generation relies on choosing whichever face is nearest, as shown in Fig.~\ref{fig::ptb1}. If the robot is closest to a cube edge or corner, the distance and direction of repulsion are handled via plane or line logic, whichever is more appropriate.
\begin{figure}[t]
\centering
\begin{subfigure}{0.48\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/field_pic/cubicfield1.pdf}
\caption{Cubic Surface Field on $\workspace^B_{S1}$}
\label{fig::ptb1}
\end{subfigure}
\begin{subfigure}{0.48\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/field_pic/cubicfield2.pdf}
\caption{Cubic Edge Field on $\workspace^B_{S2}\cap \workspace^B_{S3}$}
\label{fig::ptb2}
\end{subfigure}
\caption{Cubic force relies on the closest surface. (a) When robot closest to surface, the surface plane provides the force (Sec.~\ref{sec::planefield}. (b) When robot closest to an edge, either surface can be chosen.}% to calculate the cubic force.}
\label{fig::ptb::Force}
\end{figure}
% ######################
% ######################
% ###################### Cylinder Field
% ######################
% ######################
\subsection{\textbf{Cylinder-Based Potential Field}}
\label{sec:compute:cylinder}
%
Cylinders commonly arise from objects such as cups, bottles, cans, pipes, or cylindrical machinery components. We define a cylinder by its central axis (stemming from two vertices $\point^Y_{O1},\point^Y_{O2} {\in} \realspace$) and the radius $r^Y$.
%
% A cylinder is defined by its central axis and radius $r^Y$. The central axis has two vertex points $\point^Y_{O1,O2}\in\realspace$.
% A cylinder is defined by %has two types of surfaces: one is the
We explore the curved surface surrounding the cylinder, and two circular-cross section planes
centred at the vertices.
% at the top and bottom.
The normal direction of the curved surface is the radial direction from the central axis %, perpendicular to the surface, and
extending outward, Fig~\ref{fig::ptY::Force}. The normal %directions
of the circular surfaces are perpendicular to the plane and point outside the cylinder, %.
% \yuhe{When the robot is right above the cylinder axis, $\ndirection^Y_{OR}=\normaldirection({\point^Y_{O1},\point^R})$ is in the same direction of the axis $\ndirection^Y_{O_1O_2}=\normaldirection({\point^Y_{O1},\point^Y_{O2}})$, which we define $||\ndirection^Y_{OR}-\ndirection^Y_{O_1O_2}|| < 0.01$}\lf{$<==$ Do we need to keep it?}\yuhe{yes we can delete},
then we directly apply the force along the axis direction $\fdirection^Y_X$.
When the robot is “beside” the curved surface---as in Fig.~\ref{fig::ptb1}---the force can be computed akin to a line-based approach, whereas if it is above or below, we treat the top or bottom circle similar to the plane-like surface.
Assuming no penetration, the former occurs when the distance ($d^Y_{\bot X}$) from the robot to its perpendicular foot within the cylinder-axis is larger than the radius, whereas $d^Y_{\bot X} {<} r^Y$ indicates the robot is right above or below the cylinder (Fig.~\ref{fig::ptY2}).
To compute the resulting forces, we compute a robot-axis plane and the orthogonal vector to the cylinder axis ($\ndirection^Y_{\bot X}$)---as in Section~\ref{sec::linefield}. Note $d^Y_{\bot X}$ is computed along $\ndirection^Y_{\bot X}$.
% \lf{Yuhe, check if we can remove the following: }\lf{
% The cylinder axis and the robot define a plane, Fig.~\ref{fig::ptY::Force}. Within this robot-axis plane, the orthogonal vector to the cylinder axis is denoted as $\ndirection^Y_{\bot X}$, which can be calculated by Sec.~\ref{sec::linefield} and the distance from robot to the axis perpendicular foot can be calculated $d^Y_{\bot X}$. When the robot-axis distance larger than the radius $d^Y_{\bot X} > r^Y$, the robot is beside the cylinder (Fig.~\ref{fig::ptY1}). When the robot-axis distance is smaller than the radius $d^Y_{\bot X} < r^Y$, the robot is right above or below the cylinder (Fig.~\ref{fig::ptY2}).
% }
\noindent\textbf{Surface-Line Potential Field}
% When $d^Y_{\bot X} > r^Y$, as shown in Fig.~\ref{fig::ptY1}, the robot is beside the cylinder. The robot is
When the robot is “beside” the curved surface ($d^Y_{\bot X} > r^Y$), it is
closest to a line segment on the surface of the cylinder. To this aim, we shift the cylinder axis to the closest point of the surface by translating the vertices along the direction $\ndirection^Y_{\bot X}$, i.e.,
$\point^Y_{Si} {=} \point^Y_{Oi} + d^Y \ndirection^Y_{\bot X}$,
$ \ i{=}\{1,2\}$.
The resulting force is defined by the robot to the segment line $\overline{\point^Y_{S1}\point^Y_{S2}}$ as in Section~\ref{sec::linefield}. Thus, we have the surface-line orthogonal force $\fdirection^Y_{Xo}$, side force $\fdirection^Y_{Xs}$ and the surface-line perpendicular foot $\point^Y_{\bot X}$.
% \lf{Yuhe, check if we can remove the following: }\lf{
% The surface line is translated by the central axis along the direction of $\ndirection^Y_{\bot X}$. Alongside, the axis center points are shifted to $\point^Y_{S1,2} = \point^Y_{O1,2} + d^Y \ndirection^Y_{\bot X}$ on the surface. The line segment connecting $\point^Y_{S1}$ and $\point^Y_{S2}$ is defined as the surface line, which is closest to the robot. The surface line force is then computed following the approach in Sec.~\ref{sec::linefield}.
% }
When the perpendicular foot of surface line $\point^Y_{\bot X}$ is in between $\point^Y_{S1}$ and $\point^Y_{S2}$, the cylinder force is the orthogonal force $\fdirection^Y = \fdirection^Y_{Xo}$, which is the same as the axis orthogonal vector $\fdirection^Y_{Xo} = \ndirection^Y_{\bot X}$. When the surface line foot $\point^Y_{\bot X}$ is outside, the cylinder force is the side force $\fdirection^Y = \fdirection^Y_{Xsi}$, with $i = \{1,2\}$. The distance is computed from the robot to surface's line.
% \lf{Yuhe, is it possible to have side forces if this condition only occurs if the robot is "besides" the cylinder axis, that is, wouldn't this ensure only orthogonal forces?} \yuhe{yes for side force it also the side as the line force. So not only for orthogonal force. But the side force is also side to the circle plane - so we need to check the intersection point again as we have in plane fore. So there are three forces: orthogonal axis force, side axis force and the circular plane force when it is above the circle plane. And the only thing different is that we just need the radius to check the intersection point and the orthogonal point on circle plane. }
%
\begin{figure}[t]
\centering
\begin{subfigure}{0.58\linewidth}
\centering
\includegraphics[width=.95\linewidth]{content/field_pic/cylinder1.pdf}
\caption{Surface Line Field}
\label{fig::ptY1}
\end{subfigure}
\begin{subfigure}{0.38\linewidth}
\centering
\includegraphics[width=.95\linewidth]{content/field_pic/cylinder2.pdf}
\caption{Circular Plane Field}
\label{fig::ptY2}
\end{subfigure}
\caption{Cylinder Force has two different situations. (a) When the robot is close to the curved surface, the force can be regarded as a line force. (b) When the robot is close to the circular plane, the force can be regarded as the plane force. By checking the circular edge points, we can build the plane field on the circular plane.}
\label{fig::ptY::Force}
\end{figure}
\noindent\textbf{Circular-Plane Potential Field}
When robot-axis distance is smaller than the radius ($d^Y_{\bot X} < r^Y$), the robot remains either on the top or bottom of the cylinder (Fig.~\ref{fig::ptY2}). When the circular plane perpendicular foot is inside the plane: $\point^Y_{\bot C}\in\workspace^{Y}_C: ||\point^Y_{\bot C}-\point^Y_{O1}|| < r^Y$, we apply the circular plane force. Otherwise, it is the surface line force. The circular plane normal is the unit vector along cylinder axis $\fdirection^Y_{X} = \normaldirection(\point^Y_{O1}, \point^Y_{O2})$. Note that we have surface points $\point^Y_{S1,2}$. Suppose the robot is closer to the upper surface with the surface point $\point^Y_{S1}$. Then it is easy to find another point on the circular plane by taking the cross product $\ndirection^Y_{add}=<\ndirection^Y_{\bot X}, \fdirection^Y_{Xo}>$. Then, the four points on the edge of the circular plane are $\point^Y_{E1,3} = \point^Y_{O1} \pm d^Y \ndirection^Y_{add}$, and $\point^Y_{E2,4} = \point^Y_{O1} \pm d^Y \ndirection^Y_{\bot X}$. %\yuhe{there are three directions, one is the direction same to axis $\fdirection^Y_{X}$, one is the axis orthogonal direction $\ndirection^Y_{add}$ and the other orthogonal direction $\ndirection^Y_{\bot X} = \fdirection^Y_{Xo}$. }
% \lf{$<=$ Do we need this description?} \yuhe{we can delete if you think it is better :D}
%
With the three of them, we can shape a plane and calculate the plane force according to Sec.~\ref{sec::planefield}. However, a key distinction is that the angle-based condition for verifying the intersection point $\point^Y_{C_{\times}}$ is no longer applicable, as the original plane is circular. Instead, the intersection point can be checked by the distance. If the distance from the intersection point to the circle center exceeds the radius $\point^Y_{C_{\times}}\notin\workspace^{Y}_C: ||\point^Y_{C_{\times}}-\point^Y_{O1}|| > r^Y$, the intersection point lies outside the circular plane. Here, the distance is same as the plane-based distance for either correction or non-correction force.
\tofullversion{
The whole process is as follow:
\newline
\newline
\noindent\text{If \quad} $||\ndirection^Y_{OR}-\ndirection^Y_{O_1O_2}|| < 0.01$,
\begin{align}
(\fdirection^Y, d^Y)=(\fdirection^Y_{X},d^{Y}_{X}),
\label{CylinderforceD}
\end{align}
\text{Else} \\
\text{\quad \quad If \quad} $\point^Y_{\bot C}\in\workspace^{Y}_C: d^Y_{\bot X} < r^Y$,
\begin{align}
(\fdirection^Y, d^Y)=(\fdirection^Y_{X},d^{Y}_{X}), \quad \text{$\point^Y_{C_{\times}}\notin\workspace^{Y}_C$} \\
(\fdirection^Y, d^Y)=(\fdirection^Y_{corr},d^{Y}_{corr}), \quad \text{$\point^Y_{C_{\times}}\in\workspace^{Y}_C$}
\end{align}
\text{\quad \quad Else \quad}
\begin{align}
(\fdirection^Y, d^Y)=(\fdirection^Y_{Xo},d^{Y}_{\bot X}), \quad \text{$\point^Y_{X\bot R}\in \overline{\point^Y_{S1}\point^Y_{S2}}$}, \tag{5a}\label{CylinderforceA}\\
(\fdirection^Y, d^Y)=(\fdirection^Y_{Xsi},d^{Y}_{Xsi}), \quad \text{$\point^Y_{X\bot R}\notin \overline{\point^Y_{S1}\point^Y_{S2}}$}, \tag{5b}\label{CylinderforceB}\\
\end{align}
% \begin{empheq}[left={(\fdirection^Y, d^Y) = \empheqlbrace}]{align}
% & (\fdirection^Y_{Xo},d^{Y}_{\bot X}) & \text{$\point^Y_{X\bot R}\in \overline{\point^Y_{S1}\point^Y_{S2}}$}, \tag{5a}\label{CylinderforceA}\\
% & (\fdirection^Y_{Xsi},d^{Y}_{Xsi}) & \text{$\point^Y_{X\bot R}\notin \overline{\point^Y_{S1}\point^Y_{S2}}$}, \tag{5b}\label{CylinderforceB}\\
% & (\fdirection^Y_{X},d^{Y}_{X}) & \text{$\point^Y_{\bot C}\in\workspace^{Y}_C,\point^Y_{C_{\times}}\notin\workspace^{Y}_C$}, \tag{5c}\label{CylinderforceC}
% %& (\fdirection^Y_{Ccorr},d^{Y}_{Ccorr}) & \text{$\point^Y_{\bot C}\in\workspace^{Y}_C,\point^Y_{Cin}\notin\workspace^{Y}_C$}, \tag{5c}\label{CylinderforceD}
% \end{empheq}
}
%\lf{Yuhe: Do you think it is easy to add an begin equation (labelled equation) for the forces? Similar to what we have done in equations 5, 10, 11? If not, it is okay (we also don't have it for the geometric-cube. } \yuhe{yes i am writing. }
\begin{figure}[t]
\centering
\begin{subfigure}{1\linewidth}
\label{fig:exp:plane scenario1}
\includegraphics[width=\linewidth]{content/exp/spherization.pdf}
\caption{Comparison of GeoPF and CF Spherization in Easy (Left) and Hard (Right) Scenarios.
%The CF parameters in the plot are the one achieves $100\%$ success rate in Table.~\ref{table::Field} First Part. Black lines represent line obstacles, while black circles indicate the spherization of the lines.
Black lines and circles represent line obstacles and their spherization.
In clutter, spherization forces trajectories around all obstacles, while GeoPF navigates %through
narrow spaces between obstacles and successfully reaches the goal.}
%\caption{Line Easy Scenario}
\end{subfigure}
\begin{subfigure}{0.48\linewidth}
\centering
\includegraphics[width=0.8\linewidth]{content/exp/spherization_lineplane.pdf}
\caption{Spherization of Line and Plane}
\label{fig:exp:plane scenario1}
\end{subfigure}
\begin{subfigure}{0.48\linewidth}
\centering
\includegraphics[width=0.6\linewidth]{content/exp/planeexp.pdf}
\caption{Trajectory in Plane Scenario}
\label{fig:exp:plane scenario2}
\end{subfigure}
% \caption{Comparison of GeoPF and CF Spherization in (a) Ramdom Line and (c) Random Plane Scenarios. (b) illustrates the increased scale of objects (spheres) in spherization from lines to planes, resulting in a sharp increase in computational time. (c) While GeoPF gets a feasible path through obstacles, CF and PF are trapped in local minima, leading to a lower success rate compared to GeoPF in Plane Scenarios.}
\caption{%Comparison of GeoPF and CF
Spherization in random Line and Plane Scenes. (b) %illustrates
% the increased scale of objects (spheres) in
shows how spherization scales objects from lines to planes, sharply increasing computation time.
(c) GeoPF finds feasible paths,
%the spherization from lines to planes, resulting in a sharp increase in computational time.
%(c) While GeoPF gets a feasible path through obstacles,
CF and PF are trapped in local minima, %leading
yielding %to a
lower success rates.}
% compared to GeoPF in Plane Scenarios.}
\label{fig:exp:line spherization}
\end{figure}
% \begin{figure}[t]
% \centering
% \begin{subfigure}{0.48\linewidth}
% \centering
% \includegraphics[width=0.8\linewidth]{content/exp/spherization_lineplane.pdf}
% \caption{Spherization of Line and Plane}
% \label{fig:exp:plane scenario1}
% \end{subfigure}
% \begin{subfigure}{0.48\linewidth}
% \centering
% \includegraphics[width=0.8\linewidth]{content/exp/planeexp.pdf}
% \caption{Trajectory in Plane Scenario}
% \label{fig:exp:plane scenario2}
% \end{subfigure}
% \caption{(a) illustrates the increased scale of objects (spheres) in spherization from lines to planes, resulting in a sharp increase in computational time. (b) While GeoPF successfully get a feasible path through obstacles, CF and PF are trapped in local minima, leading to a lower success rate compared to GeoPF. %\lf{Yuhe: Highlight the init and end of the trajectory.}
% }
% \label{fig:exp:plane scenario}
% \end{figure}
\section{Quantitative Analysis and Experiments}
% \lf{Yuhe good tip on the writing: classical is like architecture or music, referring mostly to historical or a specific type, while a standard method is a classic one. ps.: Another point, we have a time component in our method (because it is based on forces, hence we have a motion/trajectory generation, not a path planning (which is like the user-guided which generates a path without the time information). Those are very common mistakes we still find all over. :)} \yuhe{ah, understood.}
% In this section, we present an extensive evaluation of our proposed GeoPF in both simulated and real-world settings.
% Our goal is twofold: (i) To quantitatively assess performance%
This section presents an extensive evaluation of GeoPF %.
with two main goals,
% Our goal is twofold:
(i) to quantitatively assess its performance%
%
% \footnote{Key performance metrics include (i) success rate—whether the robot reaches the goal without collision; (ii) computational time —the total time (or average per-step time) for generating a full trajectory; (iii)-(iv) path quality—including path length and minimum distance to obstacles. }
\footnote{Key performance %metrics
include (i) success rate—reaching the goal w/o collision, (ii) computational time; (iii)-(iv) path length and min. distance to obstacles. }
%
against %baseline reactive planners based on
state-of-the-art field-based planners across a variety of randomized scenes,
% (Subsection~\ref{sec: QuantitativeAnalysis}),
and to demonstrate GeoPF’s feasibility, % and
reliability\revised{, and robustness} in real %robotic
experiments with a Franka \revised{robot}. % Emika arm (Subsection~\ref{sec:Experiment}).
%
To ensure fairness, all experiments were run on the same hardware,\footnote{AMD Ryzen\hspace{-1pt} 7\hspace{-1pt} 5800H, 32GB\hspace{-1pt} DDR4-3200 under Ubuntu 20.04, gcc/c++\hspace{-1pt} 9.4.\negmedspace} %The randomization seeds, number of obstacles, and various obstacle positions for each scenario vary to thoroughly test each method’s robustness.
with randomized seeds, obstacle counts and placements, to thoroughly test robustness.
Unless otherwise noted, we fix the same gains for GeoPF across all trials, highlighting its minimal tuning requirement.
\subsection{\textbf{Quantitative Analysis}}
\label{sec:QuantitativeAnalysis}
%
\revised{We evaluate planners under $1000$ randomized cases per scene} within a workspace
$x,y,z {\in} [{-}0.2,0.2]$~m with initial position \revised{$[0, 1, 0]$} and goal $[0, -1, 0]$. State-of-the-art Baseline planners.
%
% We compare GeoPF with two baselines, which have been
% %
% This section provides a quantitative assessment of the performance of GeoPF and a comparison against baseline state-of-the-art field planners in a simulated environment.%\footnote{For the simulated scenarios, a collision is considered when the distance between the robot and an obstacle is less than $10^{-8}$.}
% \footnote{\revised{}}
% \textcolor{blue}{the collision is judge by $10^-8$ for numerical tolerance for simulated environments.}
%
\begin{itemize}
\item \textit{Potential Field} (PF): A standard artificial potential field with point- or sphere-based obstacle approximations;
\item \textit{Circular Field} (CF): A force-based variant exploiting circulatory vector fields along spherical obstacles~\cite{laha2021reactive}.
\end{itemize}
%
\revised{Tuning and spherization values were optimized to improve PF and CF results, under the following designed scenes. }
% We evaluate the planners under the following scenarios with randomized conditions, and different spherization strategies (optimized to improve PF and CF results).
% For each scenario, we generate $1000$ cases randomly sampling obstacles.
% All scenes were defined in a $x,y,z {\in} [{-}0.2,0.2]$~m workspace with initial \textcolor{blue}{$[0, 1, 0]$} and goal positions $[0, -1, 0]$.
\begin{itemize}
\item \textit{Line obstacles:} randomly generated lines %are generated with multiple randomly generated line obstacles, as seen in
(Fig.~\ref{fig:exp:line spherization}) classified as %We classify scenarios
as “\textit{easy}” (5–10 lines) or “\textit{hard}” (10–50 lines);
%
\item \revised{\textit{Plane obstacles:}} randomly generated planes, classified as as \textit{easy} (2-8 planes) or \textit{hard} (10-40 planes). We also consider “\textit{longer}” scenarios (changing the goal position);
% Scenarios with multiple planes are also classified as “\textit{easy}” or “\textit{hard}, according to the number of randomly generated planes, 2–8 for the former and 10-40 planes for hard scenarios. For plane-based scenarios, we also consider “\textit{longer}” scenarios (changing the goal position); %\lf{Yuhe: instead of using -h for high, can we use longer? I think it is easy to justify, otherwise it sounds odd why we took a height scenario. If you agree, then change also in the table. }
\item \textit{Maze-Scenario}: designed over classic planning task, we compared the methods over different maze conditions.
\end{itemize}
%
The results with key performance metrics% ,%
%
\footnote{%At each timestep, we record the distances between the robot and all obstacles.
The minimum distance is the smallest value across the trajectory, while the average takes all the robot-obstacle distances through the whole trajectory. }
%
%for all planners,
are shown in Table~\ref{table::Field}.\hspace{-4pt} % and further detailed below. %For the maze scenario, we only present an illustrate case in Fig.~\ref{fig::exp::tunnel}.
\noindent\textbf{Discussion and Results}
% \subsection{\lf{here:) }\textbf{Qauntitative} Analysis \lf{With time, we need to double check for typos (the footnote has another one). :) }}\yuhe{which typo? }
% % \label{sec:QuantitativeAnalysis}
% Quantitatively, we evaluate the performance of GeoAPF against Potential Field and Circular Field. We compare them on the same hardware\footnote{The computer \lf{here:) } sued in this work: AMD Ryzen 7 5800H, 32GB DDR4-3200 running Ubuntu20.04 with gcc/c++ 9.4.}.
% We randomly generate $1000$ scenarios in each task. We evaluate several features. The object number \textbf{(Ob. Num.)} indicates that how many objects in the scenarios. For the CF and PF, the object number is the sphere numbers of the spherization. The success rate \textbf{(SR)} is used to evaluate whether the robot can reach the goal position without collision. A collision is considered when the distance between the robot and an obstacle is less than $10^{-8}$. The computational time in $\mu s$ \textbf{(CT)} refers to the total time required to complete the entire trajectory, from the starting position to the goal. Additionally, we evaluate the number of steps \textbf{(Step)} the robot takes to reach the goal and the corresponding path length \textbf{(PL)}. Furthermore, we also record the minimum \textbf{(Min.)} and average \textbf{(Ave.)} distance \textbf{(Dist.)} At each timestep, we record the distances between the robot and all obstacles. The minimum distance is determined as the smallest recorded value across all steps, while the average distance is the average value of all the robot-obstacle distances through the whole trajectory.
% \subsubsection{Line Field} We design easy scenarios and hard scenarios. In the easy scenarios, 5 to 10 lines are generated, with the vertex points of each line randomly positioned within the range of $[-0.2, 0.2]$ for x, y and z axis in Cartesian space. The robot is expected to move from $[0, -1, 0]$ to $[0, 1, 0]$. In hard scenarios, 10-50 lines are randomly generated.
For \textit{Line Obstacle Scenarios},
\removed{both}
the CF and PF require approximating each
\removed{line}
segment with small spheres \removed{as seen in}
(Fig.~\ref{fig:exp:line spherization}). While small spheres ($r^{SP} = 0.01$) increase accuracy, they \revised{greatly increase the number of obstacle primitives and computation time.}
% inflate the number of obstacles (and thus computational time).
Larger spheres $r^{SP} = 0.05$ reduce \minorrev{complexity} \removed{the count} but degrade fidelity.\footnote{%
Since PF is not as competitive as CF~\cite{laha2023predictive}, we only consider $r^{SP} {=} 0.01$.
}
%
% The field parameter for
% GeoPF was set to $k^{Geo}=0.1$ across all tasks.
\revised{GeoPf used a fixed $k^{Geo}=0.1$, across all tasks.}
%For
PF and CF mainly used $k^{attr}=1$, and $k^{attr}=0.1$ occasionally improving performance.
%, we primarily used $k^{attr}=1$, although in some cases, a lower value of $k^{attr}=0.1$ led to improved performance.
For each task, we performed a limited parameter search to identify optimal values for PF and CF.
\removed{The results, in}
Table.~\ref{table::Field} \minorrev{shows GeoPF}
\removed{, show that GeoPF}
consistently obtains higher success rates \minorrev{and up to $8\times$ faster computation.}
\removed{with shorter computational times (up to $8\times$ faster).}
This is
\revised{thanks not only to its closed-form distance and force computations, but also to its ability to exploit the geometric structure of lines and planes, differentiating and modulating repulsive forces based on interactions with faces, edges, or vertices. Instead, existing methods rely solely on the closes-contact point---except for CF, which uses sphere-based modulation but introduces significant overhead, especially in cluttered scenes.}
% partly due to the closed-form distance/force computations for line segments. In contrast, CF and PF often suffer from large overheads due to sphere decompositions, especially in cluttered scenarios.
% \textbf{Easy Scenario.} Since Circular Field needs the spherization of the line, we choose different spherization radius to check the performance of circular field (CF) ($r^{SP} = 0.01$ and $r^{SP} = 0.05$). While $r^{SP} = 0.01$ guarantees the accuracy of the spherization model, $r^{SP} = 0.05$ brings the efficiency. Since Potential Field (PF) is not as competitive as CF~\cite{laha2023predictive}, we only consider PF with $r^{SP} = 0.01$. The field parameters $k^{attr}=1$, $k^{Geo}=0.1$. The results are shown in Table.~\ref{table::Field} First Part. We use \textbf{M.} $(r^{SP},k^{attr})$ to denote the method in the table.
%
% \textbf{Hard Scenario.} In the hard scenarios, we try a different attractive parameter, $k^{attr}=0.01$, in CF and PF. A smaller attractive force results in smoother movements, which enhances the success rate. However, it leads to an increase in computational time. Nevertheless, GeoAPF outperforms CF and PF both in computational time and success rate. As shown in Fig.~\ref{fig:exp:line spherization}, while CF and PF can only avoid extremely cluttered obstacles, GeoPF can find a path to go through it.
% \begin{figure}[t]
% \centering
% \begin{subfigure}{1\linewidth}
% \label{fig:exp:plane scenario1}
% \includegraphics[width=\linewidth]{content/exp/spherization.pdf}
% \caption{Comparison of GeoPF and CF Spherization in Easy (Left) and Hard (Right) Scenarios.
% %The CF parameters in the plot are the one achieves $100\%$ success rate in Table.~\ref{table::Field} First Part. Black lines represent line obstacles, while black circles indicate the spherization of the lines.
% Black lines and circles represent line obstacles and their spherization.
% In clutter, spherization forces trajectories around all obstacles, while GeoPF navigates %through
% narrow spaces between obstacles and successfully reaches the goal.}
% %\caption{Line Easy Scenario}
% \end{subfigure}
% \begin{subfigure}{0.48\linewidth}
% \centering
% \includegraphics[width=0.8\linewidth]{content/exp/spherization_lineplane.pdf}
% \caption{Spherization of Line and Plane}
% \label{fig:exp:plane scenario1}
% \end{subfigure}
% \begin{subfigure}{0.48\linewidth}
% \centering
% \includegraphics[width=0.6\linewidth]{content/exp/planeexp.pdf}
% \caption{Trajectory in Plane Scenario}
% \label{fig:exp:plane scenario2}
% \end{subfigure}
% % \caption{Comparison of GeoPF and CF Spherization in (a) Ramdom Line and (c) Random Plane Scenarios. (b) illustrates the increased scale of objects (spheres) in spherization from lines to planes, resulting in a sharp increase in computational time. (c) While GeoPF gets a feasible path through obstacles, CF and PF are trapped in local minima, leading to a lower success rate compared to GeoPF in Plane Scenarios.}
% \caption{%Comparison of GeoPF and CF
% Spherization in random Line and Plane Scenes. (b) %illustrates
% % the increased scale of objects (spheres) in
% shows how spherization scales objects from lines to planes, sharply increasing computation time.
% (c) GeoPF finds feasible paths,
% %the spherization from lines to planes, resulting in a sharp increase in computational time.
% %(c) While GeoPF gets a feasible path through obstacles,
% CF and PF are trapped in local minima, %leading
% yielding %to a
% lower success rates.}
% % compared to GeoPF in Plane Scenarios.}
% \label{fig:exp:line spherization}
% \end{figure}
% % \begin{figure}[t]
% % \centering
% % \begin{subfigure}{0.48\linewidth}
% % \centering
% % \includegraphics[width=0.8\linewidth]{content/exp/spherization_lineplane.pdf}
% % \caption{Spherization of Line and Plane}
% % \label{fig:exp:plane scenario1}
% % \end{subfigure}
% % \begin{subfigure}{0.48\linewidth}
% % \centering
% % \includegraphics[width=0.8\linewidth]{content/exp/planeexp.pdf}
% % \caption{Trajectory in Plane Scenario}
% % \label{fig:exp:plane scenario2}
% % \end{subfigure}
% % \caption{(a) illustrates the increased scale of objects (spheres) in spherization from lines to planes, resulting in a sharp increase in computational time. (b) While GeoPF successfully get a feasible path through obstacles, CF and PF are trapped in local minima, leading to a lower success rate compared to GeoPF. %\lf{Yuhe: Highlight the init and end of the trajectory.}
% % }
% % \label{fig:exp:plane scenario}
% % \end{figure}
\begin{figure}[t]
\centering
\includegraphics[width=1\linewidth]{content/exp/tunnel.pdf}
\caption{Maze Task. The robot should find a feasible path with avoiding the plane and go through the tunnel. }
\label{fig::exp::tunnel}
\end{figure}
% \begin{table*}[t]
% \centering
% \caption{Quantitative Analysis and comparison between CF, PF and GeoPF planners. Key metrics: \textbf{Obs.N.} (Number of obstacles considered), \textbf{SR} (success rate), \textbf{CT} (time for computing a full traj.), \textbf{Step} (number of iterations), \textbf{PL} (path length), \textbf{Min} and \textbf{Av}. \textbf{Dist}. (against obstacles).
% % \lf{Yuhe, can you divide this table into two (the complex, and dynamic is only for the GeoPF (and maybe showing the STD instead of variance in the table---it looks odd in the obstacles to have 25 obstacles and variance of 115 :P but with std will look better). }
% }
% % \yuhe{I changed the std, should I delete the step now? }
% % - Line Scenario / Plane Scenario / Complex Scenario}
% \label{table::Field}
% \begin{tabular}{@{}lcccccccccc@{}}
% \toprule
% \textcolor{blue}{Mean} / \textcolor{cyan}{STD} & M. ($r^{SP}, k^{attr}$) & \multicolumn{1}{c}{Obs.N.}
% & SR
% & \multicolumn{1}{c}{CT}
% & \multicolumn{1}{c}{Step}
% & \multicolumn{1}{c}{PL} & \multicolumn{1}{c}{Min. Dist.} & \multicolumn{1}{c}{Av. Dist.} \\
% \midrule
% \multirow{4}{*}{Line (easy)}
% & \textcolor{blue}{GeoPF} & \textcolor{blue}{$10$}/\textcolor{cyan}{$2.8$} & \textcolor{blue}{100\%} & \textcolor{blue}{0.32}/\textcolor{cyan}{0.08} & \textcolor{blue}{4909}/\textcolor{cyan}{62.83} & \textcolor{blue}{$2.03$}/\textcolor{cyan}{$0.03$} & \textcolor{blue}{$0.0460$}/\textcolor{cyan}{$0.01$} & \textcolor{blue}{$0.5098$}/\textcolor{cyan}{$0.014$} \\
% & CF ($0.01,1$) & 135/\textcolor{gray}{77.46} & 100\% & 1.16/\textcolor{gray}{0.3464} & 5044/\textcolor{gray}{81.69} & 2.210/\textcolor{gray}{0.044} & 0.0610/\textcolor{gray}{0.022} & 0.5040/\textcolor{gray}{0.014} \\
% & CF ($0.05,1$) & 31/\textcolor{gray}{17.78} & 99.4\% & 0.57/\textcolor{gray}{0.155} & 5140/\textcolor{gray}{118} & 2.175/\textcolor{gray}{0.158} & 0.0488/\textcolor{gray}{0.024} & 0.4772/\textcolor{gray}{0.014} \\
% & PF ($0.01,1$) & 135/\textcolor{gray}{77.46} & 99.2\% & 1.10/\textcolor{gray}{0.326} & 4860/\textcolor{gray}{14.42} & 1.998/\textcolor{gray}{0.005} & 0.0172/\textcolor{gray}{0.017} & 0.5050/\textcolor{gray}{0.014}\\
% \midrule
% \multirow{4}{*}{Line (hard)}
% & \textcolor{blue}{GeoPF} & \textcolor{blue}{$30$}/\textcolor{cyan}{$11.62$} & \textcolor{blue}{100\%} & \textcolor{blue}{0.94}/\textcolor{cyan}{0.36} & \textcolor{blue}{5025}/\textcolor{cyan}{86.04} & \textcolor{blue}{$2.097$}/\textcolor{cyan}{$0.055$} & \textcolor{blue}{$0.0316$}/\textcolor{cyan}{$0.01$} & \textcolor{blue}{$0.5047$}/\textcolor{cyan}{$0.01$} \\
% & CF ($0.01,0.1$) & 462/\textcolor{gray}{316} & 100\% & 7.65/\textcolor{gray}{2.986} & 11258/\textcolor{gray}{195} & 2.287/\textcolor{gray}{0.105} & 0.0505/\textcolor{gray}{0.026} & 0.6862/\textcolor{gray}{0.014} \\
% & CF ($0.05, 1$) & 101/\textcolor{gray}{70.71} & 88.7\% & 1.55/\textcolor{gray}{0.599} & 5270/\textcolor{gray}{126.73} & 2.287/\textcolor{gray}{0.21} & 0.0306/\textcolor{gray}{0.022} & 0.4850/\textcolor{gray}{0.014} \\
% & PF ($0.01, 0.1$) & 462/\textcolor{gray}{316} & 47\% & 6.96/\textcolor{gray}{3.13} & 11408/\textcolor{gray}{431} & 2.271/\textcolor{gray}{0.10} & 0.0781/\textcolor{gray}{0.01} & 0.6829/\textcolor{gray}{0.02}\\ \bottomrule
% \toprule
% \textcolor{blue}{Mean} / \textcolor{cyan}{STD} & \textbf{M.} ($r^{SP}, k^{SP}$) & \multicolumn{1}{c}{Obs.N.}
% & SR
% & \multicolumn{1}{c}{CT}
% & \multicolumn{1}{c}{Step}
% & \multicolumn{1}{c}{PL} & \multicolumn{1}{c}{Min. Dist.} & \multicolumn{1}{c}{Av. Dist.} \\
% \midrule
% \multirow{4}{*}{Plane (easy)}
% & \textcolor{blue}{GeoPF} & \textcolor{blue}{$5$}/\textcolor{cyan}{$2.236$} & \textcolor{blue}{97\%} & \textcolor{blue}{0.84}/\textcolor{cyan}{0.356} & \textcolor{blue}{5362}/\textcolor{cyan}{528} & \textcolor{blue}{$2.29$}/\textcolor{cyan}{$0.302$} & \textcolor{blue}{$0.037$}/\textcolor{cyan}{$0.046$} & \textcolor{blue}{$0.464$}/\textcolor{cyan}{$0.04$} \\
% & CF (0.01, 100) & 1e4/\textcolor{gray}{1e5} & 74.7\% & 88.8/\textcolor{gray}{37.53} & 5211/\textcolor{gray}{300} & 2.15/\textcolor{gray}{0.145} & 0.08/\textcolor{gray}{0.04} & 0.45/\textcolor{gray}{0.032}\\
% & PF (0.01, 100) & 1e4/\textcolor{gray}{1e4} & 54.5\% & 111.2/\textcolor{gray}{63.45} & 6324/\textcolor{gray}{1343.5} & 3.1/\textcolor{gray}{0.95} & 0.11/\textcolor{gray}{0.055} & 0.44/\textcolor{gray}{0.055}\\
% & CF (0.05, 10) & 584/\textcolor{gray}{547} & 31.5\% & 3.54/\textcolor{gray}{1.49} & 4881/\textcolor{gray}{73.03} & 2.01/\textcolor{gray}{0.03} & 0.0289/\textcolor{gray}{0.044} & 0.4179/\textcolor{gray}{0.039} \\
% \midrule
% \multirow{4}{*}{Plane (easy-longer)}
% & \textcolor{blue}{GeoPF} & \textcolor{blue}{$5$}/\textcolor{cyan}{$2.236$} & \textcolor{blue}{97.9\%} & \textcolor{blue}{0.87}/\textcolor{cyan}{0.373} & \textcolor{blue}{5525}/\textcolor{cyan}{592} & \textcolor{blue}{$2.33$}/\textcolor{cyan}{$0.232$} & \textcolor{blue}{$0.039$}/\textcolor{cyan}{$0.042$} & \textcolor{blue}{$0.48$}/\textcolor{cyan}{$0.045$} \\
% & CF (0.01, 100) & 1e4/\textcolor{gray}{1e4} & 78.9\% & 99.1/\textcolor{gray}{41.43} & 5437/\textcolor{gray}{308} & 2.25/\textcolor{gray}{0.13} & 0.069/\textcolor{gray}{0.032} & 0.453/\textcolor{gray}{0.045} \\
% & PF (0.01, 100) & 1e4/\textcolor{gray}{1e4} & 69.4\% & 89.7/\textcolor{gray}{41.94} & 5280/\textcolor{gray}{360} & 2.169/\textcolor{gray}{0.148} & 0.101/\textcolor{gray}{0.032} & 0.452/\textcolor{gray}{0.032} \\
% & PF (0.05, 100) & 584/\textcolor{gray}{548} & 60.2\% & 4.10/\textcolor{gray}{1.967} & 5281/\textcolor{gray}{374} & 2.170/\textcolor{gray}{0.152} & 0.101/\textcolor{gray}{0.017} & 0.424/\textcolor{gray}{0.037} \\\midrule
% \multirow{4}{*}{Plane (hard)}
% & \textcolor{blue}{GeoPF} & \textcolor{blue}{$25$}/\textcolor{cyan}{$10.72$} & \textcolor{blue}{59\%} & \textcolor{blue}{4.27}/\textcolor{cyan}{2.18} & \textcolor{blue}{5297}/\textcolor{cyan}{881} & \textcolor{blue}{$2.641$}/\textcolor{cyan}{$0.557$} & \textcolor{blue}{$0.0149$}/\textcolor{cyan}{$0.024$} & \textcolor{blue}{$0.5894$}/\textcolor{cyan}{$0.042$} \\
% & CF (0.01, 100) & 6e4/\textcolor{gray}{54770} & 46\% & 489/\textcolor{gray}{244.9} & 5923/\textcolor{gray}{547.7} & 2.49/\textcolor{gray}{0.293} & 0.044/\textcolor{gray}{0.028} & 0.585/\textcolor{gray}{0.0469} \\
% & CF (0.05, 10) & 3e3/\textcolor{gray}{7746} & 29\% & 19.3/\textcolor{gray}{9.62} & 5521/\textcolor{gray}{379.4} & 2.273/\textcolor{gray}{0.16} & 0.0403/\textcolor{gray}{0.0245} & 0.5390/\textcolor{gray}{0.037} \\
% & PF (0.05, 1) & 3e3/\textcolor{gray}{7746} & 11.2\% & 39.93/\textcolor{gray}{26.01} & 13533/\textcolor{gray}{2683} & 3.136/\textcolor{gray}{1.039} & 0.099/\textcolor{gray}{0.032} & 0.6324/\textcolor{gray}{0.05} \\
% \midrule
% \multirow{4}{*}{Plane (hard-longer)}
% & \textcolor{blue}{GeoPF} & \textcolor{blue}{$25$}/\textcolor{cyan}{$10.72$} & \textcolor{blue}{62.8\%} & \textcolor{blue}{4.39}/\textcolor{cyan}{2.02} & \textcolor{blue}{6108}/\textcolor{cyan}{632.5} & \textcolor{blue}{$2.72$}/\textcolor{cyan}{$0.528$} & \textcolor{blue}{$0.011$}/\textcolor{cyan}{$0.017$} & \textcolor{blue}{$0.617$}/\textcolor{cyan}{$0.059$} \\
% & CF (0.01, 100) & 6e4/\textcolor{gray}{54772} & 49.4\% & 489.4/\textcolor{gray}{244.9} & 5887/\textcolor{gray}{547.7} & 2.48/\textcolor{gray}{0.29} & 0.046/\textcolor{gray}{0.03} & 0.5852/\textcolor{gray}{0.048} \\
% & CF (0.01, 1) & 6e4/\textcolor{gray}{54772} & 17.4\% & 454/\textcolor{gray}{221.36} & 5038/\textcolor{gray}{63.88} & 2.09/\textcolor{gray}{0.048} & 0.010/\textcolor{gray}{0.0224} & 0.582/\textcolor{gray}{0.03} \\
% & PF (0.01, 100) & 6e4/\textcolor{gray}{54772} & 13.6\% & 417/\textcolor{gray}{173.2} & 6833/\textcolor{gray}{1483} & 3.45/\textcolor{gray}{1.068} & 0.099/\textcolor{gray}{0.006} & 0.572/\textcolor{gray}{0.017}\\
% \midrule
% \bottomrule
% \end{tabular}
% \end{table*}
% \begin{table*}[t]
% \centering
% \caption{Complex and Dynamic Scenario }
% % - Line Scenario / Plane Scenario / Complex Scenario}
% \label{table::Geo}
% \begin{tabular}{@{}lcccccccccc@{}}
% \toprule
% \textcolor{blue}{Ave.} / \textcolor{cyan}{Var.} & Method & \multicolumn{1}{c}{Obs.N.}
% & SR
% & \multicolumn{1}{c}{CT}
% & \multicolumn{1}{c}{Step}
% & \multicolumn{1}{c}{PL} & \multicolumn{1}{c}{Min. Dist.} & \multicolumn{1}{c}{Av. Dist.} \\ \midrule
% \multirow{1}{*}{Complex}
% & \textcolor{blue}{GeoPF} & \textcolor{blue}{$12$}/\textcolor{cyan}{$2.45$} & \textcolor{blue}{93.7\%} & \textcolor{blue}{3.56}/\textcolor{cyan}{1.37} & \textcolor{blue}{5189}/\textcolor{cyan}{424} & \textcolor{blue}{$2.248$}/\textcolor{cyan}{$.348$} & \textcolor{blue}{$0.0315$}/\textcolor{cyan}{$0.03$} & \textcolor{blue}{$0.5807$}/\textcolor{cyan}{$0.058$} \\
% \multirow{1}{*}{Dynamic (easy)}
% & \textcolor{blue}{GeoPF} & \textcolor{blue}{$\frac{15}{2}(\frac{5}{2})$}/\textcolor{cyan}{$1.1(1.05)$} & \textcolor{blue}{94.9\%} & \textcolor{blue}{2.47}/\textcolor{cyan}{0.765} & \textcolor{blue}{5299}/\textcolor{cyan}{1265} & \textcolor{blue}{$2.31$}/\textcolor{cyan}{$1.24$} & \textcolor{blue}{$0.0263$}/\textcolor{cyan}{$0.02$} & \textcolor{blue}{$0.5231$}/\textcolor{cyan}{$1.076$} \\ %\midrule
% \multirow{1}{*}{Dynamic (hard))}
% & \textcolor{blue}{GeoPF} & \textcolor{blue}{$15(7)$}/\textcolor{cyan}{$1.73(1)$} & \textcolor{blue}{83.2\%} & \textcolor{blue}{4.78}/\textcolor{cyan}{1.166} & \textcolor{blue}{5372}/\textcolor{cyan}{346} & \textcolor{blue}{$2.38$}/\textcolor{cyan}{$0.182$} & \textcolor{blue}{$0.017$}/\textcolor{cyan}{$0.0173$} & \textcolor{blue}{$0.5095$}/\textcolor{cyan}{$0.02$} \\
% \midrule
% \bottomrule
% \end{tabular}
% \end{table*}
% \begin{figure}[t]
% \centering
% %\captionsetup{font=tiny}
% \begin{subfigure}{0.65\linewidth}
% \centering
% \includegraphics[width=1\linewidth]{dynamic/spheredynamic.pdf}
% \caption{Dynamic Movements with Sphere - Front View.}
% \label{fig:spheredynamic}
% \end{subfigure}
% \begin{subfigure}{0.16\linewidth}
% \centering
% \includegraphics[width=\linewidth]{dynamic/spherego.png}
% \caption{No Sphere.}
% \label{fig:nosphererobot}
% \end{subfigure}
% \begin{subfigure}{0.16\linewidth}
% \centering
% \includegraphics[width=\linewidth]{dynamic/sphereback.png}
% \caption{With Sphere.}
% \label{fig:withsphererobot}
% \end{subfigure}
% \begin{subfigure}{0.65\linewidth}
% \centering
% \includegraphics[width=1\linewidth]{dynamic/cubicdynamic.pdf}
% \caption{Dynamic Movements with Cubic - Front View.}
% \label{fig:cubicdynamic}
% \end{subfigure}
% \begin{subfigure}{0.16\linewidth}
% \centering
% \includegraphics[width=\linewidth]{dynamic/cubicgo.png}
% \caption{No Cubic.}
% \label{fig:nocubicrobot}
% \end{subfigure}
% \begin{subfigure}{0.16\linewidth}
% \centering
% \includegraphics[width=\linewidth]{dynamic/cubicback.png}
% \caption{With Cubic.}
% \label{fig:withcubicrobot}
% \end{subfigure}
% \begin{subfigure}{0.65\linewidth}
% \centering
% \includegraphics[width=1\linewidth]{dynamic/cylinderdynamic.pdf}
% \caption{Dynamic Movements with Cylinder - Top View.}
% \label{fig:cubicdynamic}
% \end{subfigure}
% \begin{subfigure}{0.16\linewidth}
% \centering
% \includegraphics[width=\linewidth]{dynamic/cylindergo.png}
% \caption{With Cylinder.}
% \label{fig:nocubicrobot}
% \end{subfigure}
% \begin{subfigure}{0.16\linewidth}
% \centering
% \includegraphics[width=\linewidth]{dynamic/cylinderback.png}
% \caption{No Cylinder.}
% \label{fig:withcubicrobot}
% \end{subfigure}
% \caption{
% \revised{Dynamic obstacle avoidance with different primitive-specific modulation,
% %. The robot adapts its path
% in response to %moving primitives:
% sphere (a), cube (b), and cylinder (c).%, adjusting to each shape’s geometry, motion, and orientation.
% }
% \label{fig:dynamic:primitives}
% }
% %
% % \caption{\textcolor{blue}{Geometric primitive dependent dynamic obstacle avoidance. For (a)-(b), the robot moves from right-to-left in an obstacle-free setting ($t_0-t_1$), followed by a return trajectory (left-to-right, $t_1-t_4$) with different moving obstacles with different speed, a sphere in (a) and a cubic primitive in (b). In (c), the trajectory is inverted. A cylinder is introduced in the first trajectory forcing the robot to move backwards, while the returns remains obstacle-free. GeoPF modulates the motion adaptively based on the geometric structure, relative velocity, and spatial orientation of each obstacle primitive. }}
% \label{fig::dynamic}
% \end{figure}
\begin{figure}[t]
\centering
\includegraphics[width=\linewidth]{figs/dynamicObtacles.png}
\caption{
\revised{Dynamic obstacle avoidance with different primitive-specific modulation,
%. The robot adapts its path
in response to %moving primitives:
sphere (a), cube (b), and cylinder (c).%, adjusting to each shape’s geometry, motion, and orientation.
}}
\label{fig:dynamic:primitives}
\end{figure}
\begin{table*}[t]
\centering
\caption{Quantitative Analysis (\textcolor{blue!70!black}{mean}/\textcolor{blue!70!gray!70}{Std}) %and comparison
between CF, PF, and GeoPF planners.
Key metrics:
\revised{\textbf{N.Obs.Pr.} (number of obstacle primitives)},
\textbf{SR} (success rate),
% \textbf{CT} (s) (total computation time),
\textbf{Step} (iterations),
\textbf{CT/Step (s)} (computation time per step),
\textbf{PL} (path length),
\textbf{Min} and \textbf{Av}. \textbf{Dist.} (distance to obstacles).
}
% \textcolor{blue!70!black}{Mean} / \textcolor{blue!70!gray!70}{STD}
%
%
\label{table::Field}
\setlength{\tabcolsep}{9pt}
\begin{tabular}{@{}lcccccccccc@{}}
\toprule
% \textcolor{blue!70!black}{Mean} / \textcolor{blue!70!gray!70}{STD}
&
M. ($r^{SP}, k^{attr}$) &
N.Obs.Pr. &
SR &
% \multicolumn{1}{c}{CT (s)} &
\multicolumn{1}{c}{Step} &
\multicolumn{1}{c}{CT/Step (ms)} &
PL (m) &
\hspace{-10pt}Min. Dist. (m)&
\hspace{-10pt}Av. Dist. (m)\\
\midrule
% ================= LINE (easy) =================
\multirow{4}{*}{\makecell{Line\\(easy)}}
& \textcolor{blue!70!black}{GeoPF}
& \textbf{\textcolor{blue!70!black}{10}/\textcolor{blue!70!gray!70}{2.8}}
& \textcolor{blue!70!black}{\textbf{100}\%}
% & \textcolor{blue!70!black}{0.32}/\textcolor{blue!70!gray!70}{0.08}
& \textcolor{blue!70!black}{4909}/\textcolor{blue!70!gray!70}{62.83}
& \textbf{\ctperstepms{0.32}{0.08}{4909}{62.83}}
& \textcolor{blue!70!black}{2.03}/\textcolor{blue!70!gray!70}{0.03}
& \textcolor{blue!70!black}{.046}/\textcolor{blue!70!gray!70}{.001}
& \textbf{\textcolor{blue!70!black}{.510}/\textcolor{blue!70!gray!70}{.001}} \\
& CF ($0.01,1$)
& 135/\textcolor{gray}{77.46}
& \textbf{100}\%
% & 1.16/\textcolor{gray}{0.3464}
& 5044/\textcolor{gray}{81.69}
& \ctperstepms{1.16}{0.3464}{5044}{81.69}
& 2.210/\textcolor{gray}{0.044}
& .061/\textcolor{gray}{.001}
& .504/\textcolor{gray}{.001} \\
& CF ($0.05,1$)
& 31/\textcolor{gray}{17.78}
& 99.4\%
% & 0.57/\textcolor{gray}{0.155}
& 5140/\textcolor{gray}{118}
& \ctperstepms{0.57}{0.155}{5140}{118}
& 2.175/\textcolor{gray}{0.158}
& .049/\textcolor{gray}{.001}
& .477/\textcolor{gray}{.001} \\
& PF ($0.01,1$)
& 135/\textcolor{gray}{77.46}
& 99.2\%
% & 1.10/\textcolor{gray}{0.326}
& \textbf{4860/\textcolor{gray}{14.42}}
& \ctperstepms{1.10}{0.326}{4860}{14.42}
& \textbf{1.998/\textcolor{gray}{0.005}}
& .017/\textcolor{gray}{.001}
& .505/\textcolor{gray}{.001} \\
\midrule
% ================= LINE (hard) =================
\multirow{4}{*}{\makecell{Line\\(hard)}}
& \textcolor{blue!70!black}{GeoPF}
& \textbf{\textcolor{blue!70!black}{30}/\textcolor{blue!70!gray!70}{11.62}}
& \textbf{\textcolor{blue!70!black}{100\%}}
% & \textcolor{blue!70!black}{0.94}/\textcolor{blue!70!gray!70}{0.36}
& \textbf{\textcolor{blue!70!black}{5025}/\textcolor{blue!70!gray!70}{86.04}}
& \textbf{\ctperstepms{0.94}{0.36}{5025}{86.04}}
& \textbf{\textcolor{blue!70!black}{2.097}/\textcolor{blue!70!gray!70}{0.055}}
& \textcolor{blue!70!black}{.032}/\textcolor{blue!70!gray!70}{.001}
& \textcolor{blue!70!black}{.505}/\textcolor{blue!70!gray!70}{.001} \\
& CF ($0.01,0.1$)
& 462/\textcolor{gray}{316}
& 100\%
% & 7.65/\textcolor{gray}{2.986}
& 11258/\textcolor{gray}{195}
& \ctperstepms{7.65}{2.986}{11258}{195}
& 2.287/\textcolor{gray}{0.105}
& .051/\textcolor{gray}{.001}
& \textbf{.686/\textcolor{gray}{.001}} \\
& CF ($0.05, 1$)
& 101/\textcolor{gray}{70.71}
& 88.7\%
% & 1.55/\textcolor{gray}{0.599}
& 5270/\textcolor{gray}{126.73}
& \ctperstepms{1.55}{0.599}{5270}{126.73}
& 2.287/\textcolor{gray}{0.21}
& .031/\textcolor{gray}{.001}
& .485/\textcolor{gray}{.001} \\
& PF ($0.01, 0.1$)
& 462/\textcolor{gray}{316}
& 47\%
% & 6.96/\textcolor{gray}{3.13}
& 11408/\textcolor{gray}{431}
& \ctperstepms{6.96}{3.13}{11408}{431}
& 2.271/\textcolor{gray}{0.10}
& .078/\textcolor{gray}{.001}
& .683/\textcolor{gray}{.001} \\
\bottomrule
\toprule
% \textcolor{blue!70!black}{Mean} / \textcolor{blue!70!gray!70}{STD}
&
\textbf{M.} ($r^{SP}, k^{SP}$) &
\textcolor{blue}{N.Obs.Pr.} &
SR &
% \multicolumn{1}{c}{CT (s)} &
\multicolumn{1}{c}{Step} &
\multicolumn{1}{c}{CT/Step (ms)} &
PL (m) &
\hspace{-10pt}Min. Dist. (m)&
\hspace{-10pt}Av. Dist. (m)\\
\midrule
% ================= PLANE (easy) =================
\multirow{4}{*}{\makecell{Plane\\(easy)}}
& \textcolor{blue!70!black}{GeoPF}
& \textbf{\textcolor{blue!70!black}{5}/\textcolor{blue!70!gray!70}{2.236}}
& \textbf{\textcolor{blue!70!black}{97\%}}
% & \textcolor{blue!70!black}{0.84}/\textcolor{blue!70!gray!70}{0.356}
& \textcolor{blue!70!black}{5362}/\textcolor{blue!70!gray!70}{528}
& \textbf{\ctperstepms{0.84}{0.356}{5362}{528}}
& \textcolor{blue!70!black}{2.29}/\textcolor{blue!70!gray!70}{0.302}
& \textcolor{blue!70!black}{.037}/\textcolor{blue!70!gray!70}{.002}
&\textbf{ \textcolor{blue!70!black}{.464}/\textcolor{blue!70!gray!70}{.002}} \\
& CF (0.01, 100)
& 1e4/\textcolor{gray}{1e5}
& 74.7\%
% & 88.8/\textcolor{gray}{37.53}
& 5211/\textcolor{gray}{300}
& \ctperstepms{88.8}{37.53}{5211}{300}
& 2.15/\textcolor{gray}{0.145}
& .080/\textcolor{gray}{.001}
& .450/\textcolor{gray}{.001} \\
& PF (0.01, 100)
& 1e4/\textcolor{gray}{1e4}
& 54.5\%
% & 111.2/\textcolor{gray}{63.45}
& 6324/\textcolor{gray}{1343.5}
& \ctperstepms{111.2}{63.45}{6324}{1343.5}
& 3.1/\textcolor{gray}{0.95}
& .110/\textcolor{gray}{.003}
& .440/\textcolor{gray}{.003} \\
& CF (0.05, 10)
& 584/\textcolor{gray}{547}
& 31.5\%
% & 3.54/\textcolor{gray}{1.49}
& \textbf{4881/\textcolor{gray}{73.03}}
& \ctperstepms{3.54}{1.49}{4881}{73.03}
&\textbf{ 2.01/\textcolor{gray}{0.03}}
& .029/\textcolor{gray}{.002}
& .418/\textcolor{gray}{.002} \\
\midrule
% ================= PLANE (easy-longer) =================
\multirow{4}{*}{\makecell{\hspace{-28pt} Plane\\(easy-longer)}}
& \textcolor{blue!70!black}{GeoPF}
& \textbf{\textcolor{blue!70!black}{5}/\textcolor{blue!70!gray!70}{2.236}}
& \textbf{\textcolor{blue!70!black}{97.9\%}}
% & \textcolor{blue!70!black}{0.87}/\textcolor{blue!70!gray!70}{0.373}
& \textbf{\textcolor{blue!70!black}{5525}/\textcolor{blue!70!gray!70}{592}}
& \textbf{\ctperstepms{0.87}{0.373}{5525}{592}}
& \textcolor{blue!70!black}{2.33}/\textcolor{blue!70!gray!70}{0.232}
& \textcolor{blue!70!black}{.039}/\textcolor{blue!70!gray!70}{.042}
& \textbf{\textcolor{blue!70!black}{.480}/\textcolor{blue!70!gray!70}{.045}} \\
& CF (0.01, 100)
& 1e4/\textcolor{gray}{1e4}
& 78.9\%
% & 99.1/\textcolor{gray}{41.43}
& 5437/\textcolor{gray}{308}
& \ctperstepms{99.1}{41.43}{5437}{308}
& 2.25/\textcolor{gray}{0.13}
& .069/\textcolor{gray}{.002}
& .453/\textcolor{gray}{002} \\
& PF (0.01, 100)
& 1e4/\textcolor{gray}{1e4}
& 69.4\%
% & 89.7/\textcolor{gray}{41.94}
& 5280/\textcolor{gray}{360}
& \ctperstepms{89.7}{41.94}{5280}{360}
& \textbf{2.169/\textcolor{gray}{0.148}}
& .101/\textcolor{gray}{.001}
& .452/\textcolor{gray}{.002} \\
& PF (0.05, 100)
& 584/\textcolor{gray}{548}
& 60.2\%
% & 4.10/\textcolor{gray}{1.967}
& 5281/\textcolor{gray}{374}
& \ctperstepms{4.10}{1.967}{5281}{374}
& 2.170/\textcolor{gray}{0.152}
& .101/\textcolor{gray}{.001}
& .424/\textcolor{gray}{.001} \\
\midrule
% ================= PLANE (hard) =================
\multirow{4}{*}{\makecell{Plane\\(hard)}}
& \textcolor{blue!70!black}{GeoPF}
& \textbf{\textcolor{blue!70!black}{25}/\textcolor{blue!70!gray!70}{10.72}}
& \textbf{\textcolor{blue!70!black}{59\%}}
% & \textcolor{blue!70!black}{4.27}/\textcolor{blue!70!gray!70}{2.18}
& \textbf{\textcolor{blue!70!black}{5297}/\textcolor{blue!70!gray!70}{881}}
&\textbf{ \ctperstepms{4.27}{2.18}{5297}{881}}
& \textcolor{blue!70!black}{2.641}/\textcolor{blue!70!gray!70}{0.557}
& \textcolor{blue!70!black}{.015}/\textcolor{blue!70!gray!70}{.001}
& \textcolor{blue!70!black}{.589}/\textcolor{blue!70!gray!70}{.002} \\
& CF (0.01, 100)
& 6e4/\textcolor{gray}{54770}
& 46\%
% & 489/\textcolor{gray}{244.9}
& 5923/\textcolor{gray}{547.7}
& \ctperstepms{489}{244.9}{5923}{547.7}
& 2.49/\textcolor{gray}{0.293}
& .044/\textcolor{gray}{.001}
& .585/\textcolor{gray}{.002} \\
& CF (0.05, 10)
& 3e3/\textcolor{gray}{7746}
& 29\%
% & 19.3/\textcolor{gray}{9.62}
& 5521/\textcolor{gray}{379.4}
& \ctperstepms{19.3}{9.62}{5521}{379.4}
& \textbf{2.273/\textcolor{gray}{0.16}}
& .040/\textcolor{gray}{.001}
& .539/\textcolor{gray}{.003} \\
& PF (0.05, 1)
& 3e3/\textcolor{gray}{7746}
& 11.2\%
% & 39.93/\textcolor{gray}{26.01}
& 13533/\textcolor{gray}{2683}
& \ctperstepms{39.93}{26.01}{13533}{2683}
& 3.136/\textcolor{gray}{1.039}
& .099/\textcolor{gray}{.001}
& \textbf{.632/\textcolor{gray}{.001}} \\
\midrule
% ================= PLANE (hard-longer) =================
\multirow{4}{*}{\makecell{\hspace{-28pt} Plane\\(hard-longer)}}
& \textcolor{blue!70!black}{GeoPF}
& \textbf{\textcolor{blue!70!black}{25}/\textcolor{blue!70!gray!70}{10.72}}
& \textbf{\textcolor{blue!70!black}{62.8\%}}
% & \textcolor{blue!70!black}{4.39}/\textcolor{blue!70!gray!70}{2.02}
& \textcolor{blue!70!black}{6108}/\textcolor{blue!70!gray!70}{632.5}
& \textbf{\ctperstepms{4.39}{2.02}{6108}{632.5}}
& \textcolor{blue!70!black}{2.72}/\textcolor{blue!70!gray!70}{0.528}
& \textcolor{blue!70!black}{.011}/\textcolor{blue!70!gray!70}{.001}
&\textbf{ \textcolor{blue!70!black}{.617}/\textcolor{blue!70!gray!70}{.004}} \\
& CF (0.01, 100)
& 6e4/\textcolor{gray}{54772}
& 49.4\%
% & 489.4/\textcolor{gray}{244.9}
& 5887/\textcolor{gray}{547.7}
& \ctperstepms{489.4}{244.9}{5887}{547.7}
& 2.48/\textcolor{gray}{0.29}
& .046/\textcolor{gray}{.001}
& .585/\textcolor{gray}{.002} \\
& CF (0.01, 1)
& 6e4/\textcolor{gray}{54772}
& 17.4\%
% & 454/\textcolor{gray}{221.36}
&\textbf{ 5038/\textcolor{gray}{63.88}}
& \ctperstepms{454}{221.36}{5038}{63.88}
& \textbf{2.09/\textcolor{gray}{0.048}}
& .010/\textcolor{gray}{.001}
& .582/\textcolor{gray}{.001} \\
& PF (0.01, 100)
& 6e4/\textcolor{gray}{54772}
& 13.6\%
% & 417/\textcolor{gray}{173.2}
& 6833/\textcolor{gray}{1483}
& \ctperstepms{417}{173.2}{6833}{1483}
& 3.45/\textcolor{gray}{1.068}
& .099/\textcolor{gray}{.001}
& .572/\textcolor{gray}{.001} \\
\midrule
\bottomrule
\end{tabular}
\end{table*}
% \begin{table*}[t]
% \centering
% \caption{Quantitative Analysis (\textcolor{blue!70!black}{mean}/\textcolor{blue!70!gray!70}{Std}) %and comparison
% between CF, PF, and GeoPF planners.
% Key metrics:
% \revised{\textbf{N.Obs.Pr.} (number of obstacle primitives)},
% \textbf{SR} (success rate),
% % \textbf{CT} (s) (total computation time),
% \textbf{Step} (iterations),
% \textbf{CT/Step (s)} (computation time per step),
% \textbf{PL} (path length),
% \textbf{Min} and \textbf{Av}. \textbf{Dist.} (distance to obstacles).
% }
% % \textcolor{blue!70!black}{Mean} / \textcolor{blue!70!gray!70}{STD}
% %
% %
% \label{table::Field}
% \setlength{\tabcolsep}{9pt}
% \begin{tabular}{@{}lcccccccccc@{}}
% \toprule
% % \textcolor{blue!70!black}{Mean} / \textcolor{blue!70!gray!70}{STD}
% &
% M. ($r^{SP}, k^{attr}$) &
% N.Obs.Pr. &
% SR &
% % \multicolumn{1}{c}{CT (s)} &
% \multicolumn{1}{c}{Step} &
% \multicolumn{1}{c}{CT/Step (ms)} &
% PL (m) &
% \hspace{-10pt}Min. Dist. (m)&
% \hspace{-10pt}Av. Dist. (m)\\
% \midrule
% % ================= LINE (easy) =================
% \multirow{4}{*}{\makecell{Line\\(easy)}}
% & \textcolor{blue!70!black}{GeoPF}
% & \textcolor{blue!70!black}{10}/\textcolor{blue!70!gray!70}{2.8}
% & \textcolor{blue!70!black}{100\%}
% % & \textcolor{blue!70!black}{0.32}/\textcolor{blue!70!gray!70}{0.08}
% & \textcolor{blue!70!black}{4909}/\textcolor{blue!70!gray!70}{62.83}
% & \ctperstepms{0.32}{0.08}{4909}{62.83}
% & \textcolor{blue!70!black}{2.03}/\textcolor{blue!70!gray!70}{0.03}
% & \textcolor{blue!70!black}{.046}/\textcolor{blue!70!gray!70}{.001}
% & \textcolor{blue!70!black}{.510}/\textcolor{blue!70!gray!70}{.001} \\
% & CF ($0.01,1$)
% & 135/\textcolor{gray}{77.46}
% & 100\%
% % & 1.16/\textcolor{gray}{0.3464}
% & 5044/\textcolor{gray}{81.69}
% & \ctperstepms{1.16}{0.3464}{5044}{81.69}
% & 2.210/\textcolor{gray}{0.044}
% & .061/\textcolor{gray}{.001}
% & .504/\textcolor{gray}{.001} \\
% & CF ($0.05,1$)
% & 31/\textcolor{gray}{17.78}
% & 99.4\%
% % & 0.57/\textcolor{gray}{0.155}
% & 5140/\textcolor{gray}{118}
% & \ctperstepms{0.57}{0.155}{5140}{118}
% & 2.175/\textcolor{gray}{0.158}
% & .049/\textcolor{gray}{.001}
% & .477/\textcolor{gray}{.001} \\
% & PF ($0.01,1$)
% & 135/\textcolor{gray}{77.46}
% & 99.2\%
% % & 1.10/\textcolor{gray}{0.326}
% & 4860/\textcolor{gray}{14.42}
% & \ctperstepms{1.10}{0.326}{4860}{14.42}
% & 1.998/\textcolor{gray}{0.005}
% & .017/\textcolor{gray}{.001}
% & .505/\textcolor{gray}{.001} \\
% \midrule
% % ================= LINE (hard) =================
% \multirow{4}{*}{\makecell{Line\\(hard)}}
% & \textcolor{blue!70!black}{GeoPF}
% & \textcolor{blue!70!black}{30}/\textcolor{blue!70!gray!70}{11.62}
% & \textcolor{blue!70!black}{100\%}
% % & \textcolor{blue!70!black}{0.94}/\textcolor{blue!70!gray!70}{0.36}
% & \textcolor{blue!70!black}{5025}/\textcolor{blue!70!gray!70}{86.04}
% & \ctperstepms{0.94}{0.36}{5025}{86.04}
% & \textcolor{blue!70!black}{2.097}/\textcolor{blue!70!gray!70}{0.055}
% & \textcolor{blue!70!black}{.032}/\textcolor{blue!70!gray!70}{.001}
% & \textcolor{blue!70!black}{.505}/\textcolor{blue!70!gray!70}{.001} \\
% & CF ($0.01,0.1$)
% & 462/\textcolor{gray}{316}
% & 100\%
% % & 7.65/\textcolor{gray}{2.986}
% & 11258/\textcolor{gray}{195}
% & \ctperstepms{7.65}{2.986}{11258}{195}
% & 2.287/\textcolor{gray}{0.105}
% & .051/\textcolor{gray}{.001}
% & .686/\textcolor{gray}{.001} \\
% & CF ($0.05, 1$)
% & 101/\textcolor{gray}{70.71}
% & 88.7\%
% % & 1.55/\textcolor{gray}{0.599}
% & 5270/\textcolor{gray}{126.73}
% & \ctperstepms{1.55}{0.599}{5270}{126.73}
% & 2.287/\textcolor{gray}{0.21}
% & .031/\textcolor{gray}{.001}
% & .485/\textcolor{gray}{.001} \\
% & PF ($0.01, 0.1$)
% & 462/\textcolor{gray}{316}
% & 47\%
% % & 6.96/\textcolor{gray}{3.13}
% & 11408/\textcolor{gray}{431}
% & \ctperstepms{6.96}{3.13}{11408}{431}
% & 2.271/\textcolor{gray}{0.10}
% & .078/\textcolor{gray}{.001}
% & .683/\textcolor{gray}{.001} \\
% \bottomrule
% \toprule
% % \textcolor{blue!70!black}{Mean} / \textcolor{blue!70!gray!70}{STD}
% &
% \textbf{M.} ($r^{SP}, k^{SP}$) &
% \textcolor{blue}{N.Obs.Pr.} &
% SR &
% % \multicolumn{1}{c}{CT (s)} &
% \multicolumn{1}{c}{Step} &
% \multicolumn{1}{c}{CT/Step (ms)} &
% PL (m) &
% \hspace{-10pt}Min. Dist. (m)&
% \hspace{-10pt}Av. Dist. (m)\\
% \midrule
% % ================= PLANE (easy) =================
% \multirow{4}{*}{\makecell{Plane\\(easy)}}
% & \textcolor{blue!70!black}{GeoPF}
% & \textcolor{blue!70!black}{5}/\textcolor{blue!70!gray!70}{2.236}
% & \textcolor{blue!70!black}{97\%}
% % & \textcolor{blue!70!black}{0.84}/\textcolor{blue!70!gray!70}{0.356}
% & \textcolor{blue!70!black}{5362}/\textcolor{blue!70!gray!70}{528}
% & \ctperstepms{0.84}{0.356}{5362}{528}
% & \textcolor{blue!70!black}{2.29}/\textcolor{blue!70!gray!70}{0.302}
% & \textcolor{blue!70!black}{.037}/\textcolor{blue!70!gray!70}{.002}
% & \textcolor{blue!70!black}{.464}/\textcolor{blue!70!gray!70}{.002} \\
% & CF (0.01, 100)
% & 1e4/\textcolor{gray}{1e5}
% & 74.7\%
% % & 88.8/\textcolor{gray}{37.53}
% & 5211/\textcolor{gray}{300}
% & \ctperstepms{88.8}{37.53}{5211}{300}
% & 2.15/\textcolor{gray}{0.145}
% & .080/\textcolor{gray}{.001}
% & .450/\textcolor{gray}{.001} \\
% & PF (0.01, 100)
% & 1e4/\textcolor{gray}{1e4}
% & 54.5\%
% % & 111.2/\textcolor{gray}{63.45}
% & 6324/\textcolor{gray}{1343.5}
% & \ctperstepms{111.2}{63.45}{6324}{1343.5}
% & 3.1/\textcolor{gray}{0.95}
% & .110/\textcolor{gray}{.003}
% & .440/\textcolor{gray}{.003} \\
% & CF (0.05, 10)
% & 584/\textcolor{gray}{547}
% & 31.5\%
% % & 3.54/\textcolor{gray}{1.49}
% & 4881/\textcolor{gray}{73.03}
% & \ctperstepms{3.54}{1.49}{4881}{73.03}
% & 2.01/\textcolor{gray}{0.03}
% & .029/\textcolor{gray}{.002}
% & .418/\textcolor{gray}{.002} \\
% \midrule
% % ================= PLANE (easy-longer) =================
% \multirow{4}{*}{\makecell{\hspace{-28pt} Plane\\(easy-longer)}}
% & \textcolor{blue!70!black}{GeoPF}
% & \textcolor{blue!70!black}{5}/\textcolor{blue!70!gray!70}{2.236}
% & \textcolor{blue!70!black}{97.9\%}
% % & \textcolor{blue!70!black}{0.87}/\textcolor{blue!70!gray!70}{0.373}
% & \textcolor{blue!70!black}{5525}/\textcolor{blue!70!gray!70}{592}
% & \ctperstepms{0.87}{0.373}{5525}{592}
% & \textcolor{blue!70!black}{2.33}/\textcolor{blue!70!gray!70}{0.232}
% & \textcolor{blue!70!black}{.039}/\textcolor{blue!70!gray!70}{.042}
% & \textcolor{blue!70!black}{.480}/\textcolor{blue!70!gray!70}{.045} \\
% & CF (0.01, 100)
% & 1e4/\textcolor{gray}{1e4}
% & 78.9\%
% % & 99.1/\textcolor{gray}{41.43}
% & 5437/\textcolor{gray}{308}
% & \ctperstepms{99.1}{41.43}{5437}{308}
% & 2.25/\textcolor{gray}{0.13}
% & .069/\textcolor{gray}{.002}
% & .453/\textcolor{gray}{002} \\
% & PF (0.01, 100)
% & 1e4/\textcolor{gray}{1e4}
% & 69.4\%
% % & 89.7/\textcolor{gray}{41.94}
% & 5280/\textcolor{gray}{360}
% & \ctperstepms{89.7}{41.94}{5280}{360}
% & 2.169/\textcolor{gray}{0.148}
% & .101/\textcolor{gray}{.001}
% & .452/\textcolor{gray}{.002} \\
% & PF (0.05, 100)
% & 584/\textcolor{gray}{548}
% & 60.2\%
% % & 4.10/\textcolor{gray}{1.967}
% & 5281/\textcolor{gray}{374}
% & \ctperstepms{4.10}{1.967}{5281}{374}
% & 2.170/\textcolor{gray}{0.152}
% & .101/\textcolor{gray}{.001}
% & .424/\textcolor{gray}{.001} \\
% \midrule
% % ================= PLANE (hard) =================
% \multirow{4}{*}{\makecell{Plane\\(hard)}}
% & \textcolor{blue!70!black}{GeoPF}
% & \textcolor{blue!70!black}{25}/\textcolor{blue!70!gray!70}{10.72}
% & \textcolor{blue!70!black}{59\%}
% % & \textcolor{blue!70!black}{4.27}/\textcolor{blue!70!gray!70}{2.18}
% & \textcolor{blue!70!black}{5297}/\textcolor{blue!70!gray!70}{881}
% & \ctperstepms{4.27}{2.18}{5297}{881}
% & \textcolor{blue!70!black}{2.641}/\textcolor{blue!70!gray!70}{0.557}
% & \textcolor{blue!70!black}{.015}/\textcolor{blue!70!gray!70}{.001}
% & \textcolor{blue!70!black}{.589}/\textcolor{blue!70!gray!70}{.002} \\
% & CF (0.01, 100)
% & 6e4/\textcolor{gray}{54770}
% & 46\%
% % & 489/\textcolor{gray}{244.9}
% & 5923/\textcolor{gray}{547.7}
% & \ctperstepms{489}{244.9}{5923}{547.7}
% & 2.49/\textcolor{gray}{0.293}
% & .044/\textcolor{gray}{.001}
% & .585/\textcolor{gray}{.002} \\
% & CF (0.05, 10)
% & 3e3/\textcolor{gray}{7746}
% & 29\%
% % & 19.3/\textcolor{gray}{9.62}
% & 5521/\textcolor{gray}{379.4}
% & \ctperstepms{19.3}{9.62}{5521}{379.4}
% & 2.273/\textcolor{gray}{0.16}
% & .040/\textcolor{gray}{.001}
% & .539/\textcolor{gray}{.003} \\
% & PF (0.05, 1)
% & 3e3/\textcolor{gray}{7746}
% & 11.2\%
% % & 39.93/\textcolor{gray}{26.01}
% & 13533/\textcolor{gray}{2683}
% & \ctperstepms{39.93}{26.01}{13533}{2683}
% & 3.136/\textcolor{gray}{1.039}
% & .099/\textcolor{gray}{.001}
% & .632/\textcolor{gray}{.001} \\
% \midrule
% % ================= PLANE (hard-longer) =================
% \multirow{4}{*}{\makecell{\hspace{-28pt} Plane\\(hard-longer)}}
% & \textcolor{blue!70!black}{GeoPF}
% & \textcolor{blue!70!black}{25}/\textcolor{blue!70!gray!70}{10.72}
% & \textcolor{blue!70!black}{62.8\%}
% % & \textcolor{blue!70!black}{4.39}/\textcolor{blue!70!gray!70}{2.02}
% & \textcolor{blue!70!black}{6108}/\textcolor{blue!70!gray!70}{632.5}
% & \ctperstepms{4.39}{2.02}{6108}{632.5}
% & \textcolor{blue!70!black}{2.72}/\textcolor{blue!70!gray!70}{0.528}
% & \textcolor{blue!70!black}{.011}/\textcolor{blue!70!gray!70}{.001}
% & \textcolor{blue!70!black}{.617}/\textcolor{blue!70!gray!70}{.004} \\
% & CF (0.01, 100)
% & 6e4/\textcolor{gray}{54772}
% & 49.4\%
% % & 489.4/\textcolor{gray}{244.9}
% & 5887/\textcolor{gray}{547.7}
% & \ctperstepms{489.4}{244.9}{5887}{547.7}
% & 2.48/\textcolor{gray}{0.29}
% & .046/\textcolor{gray}{.001}
% & .585/\textcolor{gray}{.002} \\
% & CF (0.01, 1)
% & 6e4/\textcolor{gray}{54772}
% & 17.4\%
% % & 454/\textcolor{gray}{221.36}
% & 5038/\textcolor{gray}{63.88}
% & \ctperstepms{454}{221.36}{5038}{63.88}
% & 2.09/\textcolor{gray}{0.048}
% & .010/\textcolor{gray}{.001}
% & .582/\textcolor{gray}{.001} \\
% & PF (0.01, 100)
% & 6e4/\textcolor{gray}{54772}
% & 13.6\%
% % & 417/\textcolor{gray}{173.2}
% & 6833/\textcolor{gray}{1483}
% & \ctperstepms{417}{173.2}{6833}{1483}
% & 3.45/\textcolor{gray}{1.068}
% & .099/\textcolor{gray}{.001}
% & .572/\textcolor{gray}{.001} \\
% \midrule
% \bottomrule
% \end{tabular}
% \end{table*}
\begin{table}[t]
\centering
\caption{GeoPF performance in complex and dynamic scenarios. Average (black) and standard deviation (gray) shown. \textbf{CT/S}. represents \textbf{CT/Step}. \textbf{Min.D.} represents \textbf{Min.Dist.}}
\label{table::Geo_compact}
\begin{tabular}{@{}lcccc@{}}
\toprule
\textbf{Scenario} & \textbf{N.Obs.Pr.} & \textbf{SR} & \textbf{CT/S. (ms)} & \textbf{\hspace{-2pt}Min.\hspace{-1pt} D. \hspace{-2pt}(m)} \\
\midrule
Complex & \textcolor{black}{$12$}/\textcolor{gray}{$2.45$} & \textcolor{black}{93.7\%} & \textbf{\textcolor{black}{$0.62$}}/\textcolor{gray}{$0.37$} & \textcolor{black}{$0.032$}/\textcolor{gray}{$0.03$} \\
Dynamic (easy) & \textcolor{black}{$\frac{15}{2}(\frac{5}{2})$}/\textcolor{gray}{$1.1(1.05)$} & \textcolor{black}{94.9\%} & \textbf{\textcolor{black}{$0.48$}}/\textcolor{gray}{$0.07$} & \textcolor{black}{$0.026$}/\textcolor{gray}{$0.02$} \\
Dynamic (hard) & \textcolor{black}{$15(7)$}/\textcolor{gray}{$1.73(1)$} & \textcolor{black}{83.2\%} & \textbf{\textcolor{black}{$0.89$}}/\textcolor{gray}{$0.25$} & \textcolor{black}{$0.017$}/\textcolor{gray}{$0.02$} \\
\bottomrule
\end{tabular}
\end{table}
% \subsubsection{Plane Field}
For \textit{Plane-Obstacle Scenarios},\footnote{\revised{New parameter search was required for PF and CF, as prior values (used successfully in line-based scenes) consistently failed. We analyzed repulsive coefficients $k^{SP}=1,10,100$. GeoPF kept the same parameters.}}
%
spherization led to massive sphere counts and excessive computational times. Except in simple cases, PF and CF could not produce feasible solutions within real-time constraints, as shown in Table~II. In contrast, GeoPF reduced computational time by up to $100\times$, achieving real-time (below 1~ms) in all cases while maintaining higher success rates even in dense and cluttered scenarios.
%
%
\removed{we conducted a new parameter search to identify optimal values for PF and CF, as the previously used parameters consistently led to failure. In this evaluation, we analyzed different repulsive coefficients for PF and CF, $k^{SP}=1,10,100$. GeoPF uses the same parameters as in the line task, with $k^{attr}=1$, $k^{Geo}=0.1$.
The results indicate that plane spherization generates a significantly large number of spheres, resulting in an extremely high computational time.
Indeed, except for simple line-based scenes, CF and PF planners fail to generate a feasible solution within the real-time framework, as seen in the CT/Step (ms) column in Table~\ref{table::Field}.
In contrast,
GeoPF drastically lowers the total computational time---up to $100\times$ faster in almost all cases---thanks to direct distance-to-plane calculations. It is also able to keep the real-time performance (computational below 1~ms) for all cases,11 while maintaining a higher success rate in more cluttered arrangements. %In contrast, PF and CF can get stuck behind planes (local minima) or slow down significantly due to the large number of spheres.
}
% Similarly, we design both easy and hard scenarios for plane field analysis. The easy tasks consist of 2 to 8 planes, while the hard tasks include 10 to 40 planes. Additionally, we consider different goal positions: one is same as the line tasks at $[0,1,0]$, and the other at a higher position $[0,1,0.5]$. The starting position is still $[0, -1, 0]$. In plane tasks, we evaluate the effect of different repulsive coefficients $k^{SP}=1,10,100$ for CF and PF and $k^{attr}=0.1$. GeoPF uses the same set of parameters as line task $k^{attr}=1$, $k^{Geo}=0.1$. \textbf{M.} ($r^{SP}, k^{SP}$) in Table.~\ref{table::Field}
% Second Part denotes the spherization radius and the $k$ values for spherization. The $-h$ represents the task with the higher goal position. The results indicate that plane spherization generates a significantly large number of spheres, resulting in an extremely high computational time. In contrast, GeoPF ensures a more efficient computational process even in complex scenarios. Although the success rate decreases in complex tasks, GeoPF still guarantees the best performance among the evaluated methods.
% \textbf{Maze Task}. Specifically, we design a classical path planning task, the maze task, in which the system must
\removed{
The classic \textit{Maze Task} scenario, where the robot must find a feasible path across multiple planes positioned either vertically or horizontally, is considered a standard trap case.
% find a feasible path across multiple planes---positioned either vertically or horizontally---is considered a standard trap case.
As shown in Fig.~\ref{fig::exp::tunnel}, while CF and PF fail at the centre point in front of the middle plane, GeoPF successfully navigates through the maze. This result reinforces the findings in Table~\ref{table::Field} which show that CF and PF exhibit significantly lower success in complex scenarios.}
\minorrev{In the challenging \textit{Maze Task}, considered a standard trap case for reactive planners with multiple vertical and horizontal planes (Fig.~\ref{fig::exp::tunnel}), }
CF and PF fail at the centre point in front of the middle plane, GeoPF successfully navigates through the maze, aligning with results from Table~\ref{table::Field}.
\noindent \textbf{Complex and Dynamic Scenes}
%
We
\removed{additionally tested GeoPF}
\minorrev{further evaluated}
in challenging environments composed of multiple \removed{geometric }primitives---including lines, planes, cubes, and cylinders\minorrev{, in cluttered and dynamic configurations.}\removed{---all placed or moving in random configurations. In these trials, only GeoPF was evaluated, as our goal was to demonstrate its versatility in cluttered, complex, and dynamic scenarios. % rather than compare it against spherization-based methods.
}
%
\removed{Specifically, each}
\minorrev{Each}
%%complex
composite scene randomly \removed{selected}\minorrev{included} 5–10 lines, 2–5 planes, and 2–3 cubes or cylinders distributed around the workspace.
%------%------%------
\removed{Some obstacles were static, while others followed randomized trajectories (e.g., drifting planes or cylinders).
In Table~\ref{table::Field}, the notation \textcolor{blue}{\textbf{N.Obs.Pr.}} is represented as $a(b)$, where $a$ denotes the total \textcolor{blue}{number of obstacle primitives}, while (b) the dynamic ones.
}
%------%------%------
\removed{Throughout these tests, GeoPF maintained}
\minorrev{GeoPF consistently achieved} high success rates without requiring special tuning or re-initialization, see Table~\ref{table::Geo_compact}.
\revised{Fig.~\ref{fig:dynamic:primitives} further shows the dynamic response modulated per primitive in a back-and-forth scene (with dynamic obstacles appearing in the middle of an originally straight line goal).}
The same $k$ value parameters were used \revised{throughout this paper}.
%
\revised{These results highlight GeoPF’s flexibility, real-time performance, and stable behavior in complex, dynamic scenes.}
\removed{
These results confirm the flexibility and easiness to use, as well as the reliability of GeoPF which preserves its real-time efficiency in more complex scenes with a stable and reliable behaviour.
}
% but also naturally extends to dynamic contexts, where it continues to avoid obstacles via the same closed-form geometric computations.
% The object field such as cubic field and cylinder field are based on lines and planes. We test our method in different complex scenarios which contains all the geometic primitives, as shown in Table~\ref{table::Field} Third Part. GeoPF achieves a high success rate while guarantees the computation time of the whole trajectory within the scale of microseconds.
% \subsubsection{Dynamic Field}
% In a dynamic scenario, the robot interacts with moving objects in its environment, as illustrated in Fig.~\ref{exp::fig::dynamic}. In Table~\ref{table::Field}, the notation \textbf{Ob. Num.} is represented as $a(b)$, where $a$ denotes the total number of random objects including all geometric primitives, and $b$ specifies the number of moving objects within the scenario. All scenarios have the same set of $k$ value parameters in GeoPF, which proves that GeoPF is stable and robust against different obstacles and complex scenarios.
% \begin{figure}[t]
% \centering
% \begin{subfigure}{0.49\linewidth}
% \centering
% \includegraphics[width=1\linewidth]{content/exp/complexmatlab.pdf}
% \caption{GPs Field}
% \label{exp::fig::complex1}
% \end{subfigure}
% \begin{subfigure}{0.49\linewidth}
% \centering
% \includegraphics[width=1\linewidth]{content/exp/complexmatlab.pdf}
% \caption{Dynamic GPs Field }
% \label{exp::fig::complex2}
% \end{subfigure}
% \caption{(a) illustrates the increased scale of objects (spheres) in spherization from lines to planes, resulting in a sharp increase in computational time. (b) While GeoPF successfully get a feasible path through obstacles, CF and PF are trapped in local minima, leading to a lower success rate compared to GeoPF. }
% \label{exp::fig::complex}
% \end{figure}
\removed{
\begin{figure}[t]
\centering
\includegraphics[width=1\linewidth]{content/exp/dynamicexp.pdf}
\caption{
%
{Dynamic scenarios with plane moving upwards during execution.}
% Non-stationary Scenario. The orange plane moves upward over time. The trajectory passes through the initial plane position as shown in the dark orange area. However, since the plane moves upward along the trajectory evolution, it is no longer an obstacle between robot and goal towards the end. Consequently, the robot follows a (straight) line trajectory in last period of the task.
}
\label{exp::fig::dynamic}
\end{figure}
}
\subsection{\textbf{Real Robot Experiment}}
\label{sec:Experiment}
% %%%%%%%%%%%%%%%%%%%%%%%%%%
To validate GeoPF's \removed{the} practical viability, % of GeoPF,
we deployed it on a 7-DoF Franka Robot. % robotic arm.
End-effector velocities were generated by mapping the %resultant
GeoPF forces into joint-space
via Jacobian pseudoinverse.
% commands using a standard Jacobian pseudoinverse.
Obstacles were tracked via a motion-capture system (Nokov Mars) and fitted to corresponding geometric primitives. GeoPF was tested % evaluated
across \revised{four}
% three
representative tasks.
First (Fig.~\ref{exp::fig::real1}),
in a tabletop scenario,
% (i.e., constrained z-axis), the robot navigated between table corners while avoiding random lines, cubes, and cylinders
GeoPF leveraged geometric primitives to produce
% by leveraging the geometric shapes, GeoPF produced
smooth, collision-free trajectories under rigid (z-axis) constraints\revised{, avoiding randomly placed lines, cubes, and cylinders within plane-bounded workspaces.}
%
\revised{
To further evaluate robustness to primitive shaping, we revised the same experiment, but using only bounding boxes (from off-the-shelf algorithm, e.g., YOLOv7).
As seen in Fig.~\ref{fig:real:robustness}(b), even such coarse approximations suffice for GeoPF to generate safe, efficient and real-time motions in clutter, while state-of-the-art methods Fig.~\ref{fig:real:robustness}(c)-(d) failed despite access to full geometry. }
% the robot navigated between table corners while avoiding random lines, cubes, and cylinders within fixed workspace boundaries (planes).
% a tabletop scenario, where the robot moved from one corner of the table to another while avoiding randomly placed objects. By leveraging the geometric shapes, GeoPF produced smooth, collision-free trajectories under rigid constraints (fixed z-axis), well-suited for control-based execution.
% Second, in a 3D task, the robot avoided a shelf decomposed into planar elements along with additional obstacles, generating environment-aware trajectories.
% The second task (Fig.~\ref{exp::fig::real2}) involved a 3D setting, navigating
Second, in a 3D task, the robot
navigated around a shelf decomposed into multiple planar elements, along with additional obstacles. GeoPF successfully avoided all obstacles by generating environment-aware trajectories.
% Lastly, in
In the
dynamic scene shown in Fig.~\ref{fig:teaser},
% the robot must pour tea into a moving cup while avoiding randomly moving obstacles. Here, GeoPF updated repulsive forces in real time, consistently maintaining safe distances without parameter adjustment. Indeed, across all experiments, GeoPF generated stable, smooth trajectories within real-time cycles without tuning between scenarios.
%
the robot performed tasks like pouring tea into a moving cup or navigating around moving obstacles, with GeoPF consistently maintaining safe distances in real time without parameter adjustment. Across all cases, GeoPF produced real-time smooth, collision-free trajectories suitable for control-based execution.
\revised{
Lastly, we tested GeoPF for simultaneous EE and whole-body collision avoidance (Fig.~\ref{fig::wholebody:avoidance}). The links were shaped as cylinders, actively modulating reactive forces as detailed in Sec.~\ref{sec:compute:cylinder}. The resulting forces were then mapped to torques based on the moment arm to the previous joint. This enabled coordinated, geometry-aware avoidance across the entire kinematic chain without additional tuning.
% where not only the end-effector but also the arm links—modeled as cylinders—actively avoided obstacles. Unlike end-effector-only strategies, GeoPF modulated both arm and end-effector motion in a coordinated, geometry-aware manner, successfully preventing collisions across the entire kinematic chain without additional tuning.
}
% We evaluate GeoPF using a Franka Emika Arm across three types of tasks: 3D tasks, 2D tasks, and dynamic tasks. In 2D task, the robot should find a feasible path between obstacles, as shown in Fig.~\ref{exp::fig::real1} successfully goes from the left corner of the table to the right corner. In 3D task, the robot is supposed to go over the planes while avoiding objects on the table and reaching the other side. The objects are detected using the Nokov Mars system and represented by their corresponding geometric primitives. Besides, we also test in dynamic scenario as shown in Fig.~\ref{fig:teaser}. The robot successfully avoids a moving object (the red lid long cup) while tracking a moving goal (the tea cup) and accurately pours tea into it.
\begin{figure}[t]
\centering
\begin{subfigure}{0.49\linewidth}
\centering
\includegraphics[width=0.95\linewidth]{content/exp/real2d_compressed.png}
\caption{2D Task in Real Robot}
\label{exp::fig::real1}
\end{subfigure}
\begin{subfigure}{0.49\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/exp/2Dtraj.pdf}
\caption{Trajectory for 2D Task}
\label{exp::fig::traj1}
\end{subfigure}
\begin{subfigure}{0.49\linewidth}
\centering
\includegraphics[width=0.95\linewidth]{content/exp/real3d_compressed.png}
\caption{3D Task in Real Robot}
\label{exp::fig::real2}
\end{subfigure}
\begin{subfigure}{0.49\linewidth}
\centering
\includegraphics[width=0.7\linewidth]{content/exp/3Dtraj.pdf}
\caption{Trajectory for 3D Task}
\label{exp::fig::traj1}
\end{subfigure}
\caption{Real world experiments. The robot goes from left %(low-transparent position)
to right (high-opacity) and then returns, see trajectories (b),(d). The \textit{upper figures} show a 2D tabletop task with multiple obstacles, as lines, cubes, and cylinders. The workspace
%limitation
is shown by planes, % positioned in front of and behind the table,
% defining
w.r.t. the robot's boundaries.
The \textit{bottom figures} illustrate a 3D task with four lids representing walls that the robot must avoid during navigation.
}
\label{exp::fig::real}
\end{figure}
\begin{figure}[t]
\centering\vspace{-10pt}
%\captionsetup{font=tiny}
\begin{subfigure}{0.24\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/revised/geopf.pdf}
\vspace{-20pt}\caption{\hspace{30pt} }
% \caption{\scriptsize GeoPF with full access to the primitive information}
\label{fig:GeoPF}
\end{subfigure}
\begin{subfigure}{0.24\linewidth}
\centering
\includegraphics[width=\linewidth]{content/revised/cubicization.pdf}
\vspace{-20pt}\caption{\hspace{30pt} }
% \caption{\scriptsize GeoPF w/ less privileged inform. (only bounding boxes)}
\label{fig:Cubic}
\end{subfigure}
\begin{subfigure}{0.24\linewidth}
\centering
\includegraphics[width=\linewidth]{content/revised/cf.pdf}
\vspace{-20pt}\caption{\hspace{30pt} }
% \caption{\scriptsize Circular Fields (CF) w/ full privileged primitive info.}
\label{fig:CF}
\end{subfigure}
\begin{subfigure}{0.24\linewidth}
\centering
\includegraphics[width=\linewidth]{content/revised/pf.pdf}
% \caption{\scriptsize APF with privileged primitive info.}
\vspace{-20pt}\caption{\hspace{30pt} }
\label{fig:PF}
\end{subfigure}
% \caption{Comparison of GeoPF with closest point CF / PF. (a) GeoPF in avoiding the geometric objects. (b) Use cubic to represent the objects. (c) finding the closest points on the surface and use Circular Field. (d) finding the closest points on the surface and using the potential field. }
% \caption{\textcolor{blue}{Comparison between reactive planners in a realistic real-world setting in Fig.~\ref{exp::fig::real}(a). \textbf{(a)} GeoPF with structured, primitive-fitting information, generates a high-quality trajectory that differentiate interaction based on primitive type and features;
% The same information is given to state-of-the-art and traditional reactive planners in \textbf{(c)} and \textbf{(d)}. However, both CFs and APFs --- while being given full geometric-information --- compute their avoidance strategy solely based on the closest point (ignoring the structure of the obstacle). In contrast, \textbf{(b)} depicts GeoPF with only access to bounding boxes, e.g., from YOLOv7. Even with less privileged information, i.e., poor geometric fitting, it is still comptues a feasible path, in contrast to CFs and APFs. }}
%
%
\caption{\revised{Robustness to primitive fitting---based on 2D Task (Fig.~\ref{exp::fig::real}(a)).
% The full structure primitive-fitting information is given to GeoPF (a), to CFs (c) and to APFs (d).
\textbf{(a)} GeoPF with structured primitives-fitting produces high-quality, shape-aware trajectories, while CFs \textbf{(c)} and APFs \textbf{(d)} fail to generate a collision-free trajectory, despite being given full geometric-information. \textbf{(b)} GeoPF with only access to bounding boxes still finds a feasible path---even with less privileged information. The red lines indicate that robot goes into the obsctale.
%
% \textbf{(b)} GeoPF with bounding-box input still finds a feasible path. (c) CF and (d) APF fail despite full geometry, relying only on closest-point distances.
}
}
\label{fig:real:robustness}
\end{figure}
\begin{figure}[t]
\centering
%\captionsetup{font=tiny}
\begin{subfigure}{0.32\linewidth}
\centering
\includegraphics[width=1\linewidth]{content/fullbody/noavoidance2.png}\vspace{-5pt}
\caption{\scriptsize{Reactive (no avoidance)}}
\label{fig:noavoi2}
\end{subfigure}
\begin{subfigure}{0.32\linewidth}
\centering
\includegraphics[width=\linewidth]{content/fullbody/eeavoidance2.png}\vspace{-5pt}
\caption{\scriptsize{GeoPF (w/o whole-body)}}
\label{fig:ee2}
\end{subfigure}
\begin{subfigure}{0.32\linewidth}
\centering
\includegraphics[width=\linewidth]{content/fullbody/geopf2.png}\vspace{-5pt}
\caption{\scriptsize{GeoPF w/ whole-body)}}
\label{fig:fullbody2}
\end{subfigure}
\vspace{-2pt}
\caption{%
\revised{Whole-body collision avoidance with GeoPF. (a) Planner with no avoidance, naturally leading to multiple collisions, (b) End-effector-only avoidance fails to avoid elbow collisions. (c) GeoPF's whole-body geometric-aware collision avoidance successfully modulates arm and end-effector trajectories to safely avoid all obstacles.}
}\vspace{-5pt}
\label{fig::wholebody:avoidance}
\end{figure}
% Fig. 13: Whole-body avoidance with GeoPF. (a) No avoidance leads to collisions. (b) End-effector-only avoidance fails to avoid elbow collisions. (c) GeoPF's whole-body geometric-aware collision avoidance successfully modulates arm and end-effector trajectories to safely avoid all obstacles.
\section{Conclusion} \label{sec:conclusion}
% This work introduces a novel reactive system based on geometric primitives. We propose the Geometric Potential Field framework, in which virtual forces are derived from the geometric shapes of different real-world object primitives. This enables reactive planning in non-trivial scenarios. GeoPF is evaluated through both simulations and real-world robotic experiments.
% %
% This work introduces a novel reactive planner (GeoPF) that unifies spatial geometric primitives under a temporal potential-field framework for real-time planning. By explicitly leveraging closed-form distance functions for spheres, lines, planes, cubes, and cylinders, GeoPF delivers substantial improvements over existing potential-field methods in computational efficiency, tuning simplicity, and collision avoidance performance.
% Indeed,
% quantitative analysis and experimental results demonstrate that
% GeoPF consistently outperforms existing potential-field-based methods in terms of
% reliability, ease of deployment, and computational complexity---being the only method suitable for real-time ($1$ kHz) performance in cluttered, unstructured dynamic settings.
% % our approach outperforms previous reactive methods and current state-of-the-art such as circular fields, artificial potential fields, thus exhibiting superior fast, stable, robust, and flexible performance in diverse tasks.
% Future works will involve the integration of prediction modules for obstacle tracking and multi-agent vector fields for effective environment exploration.
This work introduces GeoPF, a novel reactive planner that unifies spatial structured geometric primitives under a temporal potential-field framework for real-time planning. By explicitly leveraging closed-form distance functions \revised{and structural and spatial properties of primitives,} % for spheres, lines, planes, cubes, and cylinders,
GeoPF delivers substantial improvements %over existing potential-field methods
in computational efficiency, tuning simplicity, and collision avoidance performance.
%
%
Experiments demonstrate its robustness across static and dynamic scenarios, including whole-body avoidance, with minimal parameter adjustment.
%
Quantitative analysis and experiments demonstrate \revised{its reliability, robustness, and easy-deployment over existing field-based planners}, being the only suitable for real-time ($1$ kHz) performance in cluttered, unstructured dynamic settings. \revised{GeoPF has also been tested for whole-body avoidance and limited primitive-fitting access, all with no tuning.} %, i.e., the same parameter value.}
% GeoPF consistently outperforms existing potential-field-based methods in terms of
% reliability, ease of deployment, and computational complexity---being the only method suitable for real-time ($1$ kHz) performance in cluttered, unstructured dynamic settings.
%
%
% our approach outperforms previous reactive methods and current state-of-the-art such as circular fields, artificial potential fields, thus exhibiting superior fast, stable, robust, and flexible performance in diverse tasks.
% Future works will involve the integration of prediction modules for obstacle tracking and multi-agent vector fields for effective environment exploration---aiming to cope one limitation of GeoPF, and any reactive planner, in terms of its completeness. We also aim to integrate GeoPF primitive structure with neural SDFs, e.g., \cite{staroverov2024dynamic}, extending ANN inference to geometric descriptors.
Future work will focus on integrating prediction modules for dynamic obstacle tracking and multi-agent vector fields to address a core limitation of GeoPF—and all reactive planners—its lack of completeness. We also aim to fuse GeoPF’s primitive structure with neural SDFs (e.g.,~\cite{staroverov2024dynamic}), extending ANN inference to produce rich geometric descriptors.
% automatically infer not only distance values but also geometric descriptors such as normal direc-
% tions, primitive types, and key structural features.
% Despite these strengths, GeoPF shares the inherent limitations of reactive planners: no global convergence guarantees and reliance on geometric abstractions, which may require fitting steps in perception pipelines. Future work will focus on integrating prediction modules for dynamic obstacle tracking, extending to multi-agent settings, and combining GeoPF with global or learning-based strategies for hybrid planning.
% Generated by IEEEtran.bst, version: 1.14 (2015/08/26)
\begin{thebibliography}{10}
\providecommand{\url}[1]{#1}
\csname url@samestyle\endcsname
\providecommand{\newblock}{\relax}
\providecommand{\bibinfo}[2]{#2}
\providecommand{\BIBentrySTDinterwordspacing}{\spaceskip=0pt\relax}
\providecommand{\BIBentryALTinterwordstretchfactor}{4}
\providecommand{\BIBentryALTinterwordspacing}{\spaceskip=\fontdimen2\font plus
\BIBentryALTinterwordstretchfactor\fontdimen3\font minus \fontdimen4\font\relax}
\providecommand{\BIBforeignlanguage}[2]{{%
\expandafter\ifx\csname l@#1\endcsname\relax
\typeout{** WARNING: IEEEtran.bst: No hyphenation pattern has been}%
\typeout{** loaded for the language `#1'. Using the pattern for}%
\typeout{** the default language instead.}%
\else
\language=\csname l@#1\endcsname
\fi
#2}}
\providecommand{\BIBdecl}{\relax}
\BIBdecl
\bibitem{orthey2023sampling}
A.~Orthey, C.~Chamzas, and L.~E. Kavraki, ``Sampling-based motion planning: A comparative review,'' \emph{Annual Review of Control, Robotics, and Autonomous Systems}, vol.~7, 2023.
\bibitem{kappler2018real}
D.~Kappler, F.~Meier, J.~Issac, J.~Mainprice, C.~G. Cifuentes, M.~W{\"u}thrich, V.~Berenz, S.~Schaal, N.~Ratliff, and J.~Bohg, ``Real-time perception meets reactive motion generation,'' \emph{IEEE RAL}, 2018.
\bibitem{laha2023predictive}
R.~Laha, M.~Becker, J.~Vorndamme, J.~Vrabel, L.~F. Figueredo, M.~A. M{\"u}ller, and S.~Haddadin, ``Predictive multi-agent-based planning and landing controller for reactive dual-arm manipulation,'' \emph{IEEE TRO}, 2023.
\bibitem{du2010robotic}
N.~E. Du~Toit and J.~W. Burdick, ``Robotic motion planning in dynamic, cluttered, uncertain environments,'' in \emph{2010 IEEE International Conference on Robotics and Automation}.\hskip 1em plus 0.5em minus 0.4em\relax IEEE, 2010, pp. 966--973.
\bibitem{marcucci2024fast}
T.~Marcucci, P.~Nobel, R.~Tedrake, and S.~Boyd, ``Fast path planning through large collections of safe boxes,'' \emph{IEEE Trans. on Robotics}, 2024.
\bibitem{bryson2001modularity}
J.~J. Bryson and L.~A. Stein, ``Modularity and design in reactive intelligence,'' in \emph{Int. Joint Conf. on Artificial Intelligence (IJCAI)}, 2001.
\bibitem{DONALD1987295}
B.~R. Donald, ``A search algorithm for motion planning with six degrees of freedom,'' \emph{Artificial Intelligence}, vol.~31, no.~3, pp. 295--353, 1987.
\bibitem{correll2018analysis}
N.~Correll, K.~E. Bekris, D.~Berenson, O.~Brock, A.~Causo, K.~Hauser, K.~Okada, A.~Rodriguez, J.~M. Romano, and P.~Wurman, ``Analysis and observations from the first amazon picking challenge,'' \emph{IEEE Transactions on Automation Science and Engineering}, vol.~15, no.~1, Jan. 2018.
\bibitem{kortenkamp2016robotic}
D.~Kortenkamp, R.~G. Simmons, and D.~Brugali, ``Robotic systems architectures and programming,'' in \emph{Springer Handbook of Robotics}.\hskip 1em plus 0.5em minus 0.4em\relax New York, NY, USA: Springer-Verlag, 2016, pp. 283--306.
\bibitem{lavalle2001randomized}
S.~M. LaValle and J.~J. Kuffner~Jr, ``Randomized kinodynamic planning,'' \emph{The international journal of robotics research}, vol.~20, no.~5, 2001.
\bibitem{marcucci2023motion}
T.~Marcucci, M.~Petersen, D.~von Wrangel, and R.~Tedrake, ``Motion planning around obstacles with convex optimization,'' \emph{Science robotics}, vol.~8, no.~84, 2023.
\bibitem{Latombe1991}
J.-C. Latombe, \emph{Potential Field Methods}.\hskip 1em plus 0.5em minus 0.4em\relax Boston,\hspace{-2pt} MA: Springer \hspace{-2pt}US, 1991.
\bibitem{Warren1990}
C.~Warren, ``Multiple robot path coordination using artificial potential fields,'' in \emph{Proceedings., IEEE ICRA}, 1990.
\bibitem{koren1991potential}
Y.~Koren, J.~Borenstein \emph{et~al.}, ``Potential field methods and their inherent limitations for mobile robot navigation.'' in \emph{ICRA}, vol.~2, 1991.
\bibitem{singh1996real}
L.~Singh, H.~Stephanou, and J.~Wen, ``Real-time robot motion control with circulatory fields,'' in \emph{IEEE Intern. Conf. Robot. \& Autom.}, 1996.
\bibitem{haddadin2011dynamic}
S.~Haddadin, R.~Belder, and A.~Albu-Sch{\"a}ffer, ``Dynamic motion planning for robots in partially unknown environments,'' \emph{IFAC}, 2011.
\bibitem{becker2023motion}
M.~Becker, J.~K{\"o}hler, S.~Haddadin, and M.~A. M{\"u}ller, ``Motion planning using reactive circular fields: A 2-d analysis of collision avoidance and goal convergence,'' \emph{IEEE TAC}, vol.~69, no.~3, pp. 1552--1567, 2023.
\bibitem{hussein2002motion}
A.~M. Hussein and A.~Elnagar, ``Motion planning using maxwell's equations,'' in \emph{IEEE/RSJ IROS}.\hskip 1em plus 0.5em minus 0.4em\relax IEEE, 2002.
\bibitem{vallivaara2011magnetic}
I.~Vallivaara, J.~Haverinen, A.~Kemppainen, and J.~R{\"o}ning, ``Magnetic field-based slam method for solving the localization problem in mobile robot floor-cleaning task,'' in \emph{ICAR}.\hskip 1em plus 0.5em minus 0.4em\relax IEEE, 2011.
\bibitem{ataka2022magnetic}
A.~Ataka, H.-K. Lam, and K.~Althoefer, ``Magnetic-field-inspired navigation for robots in complex and unknown environments,'' \emph{Frontiers in Robotics and AI}, vol.~9, p. 834177, 2022.
\bibitem{todorov2012mujoco}
E.~Todorov, T.~Erez, and Y.~Tassa, ``Mujoco: A physics engine for model-based control,'' in \emph{2012 IEEE/RSJ International Conference on Intelligent Robots and Systems}, 2012, pp. 5026--5033.
\bibitem{montaut2024gjk++}
L.~Montaut, Q.~Le~Lidec, V.~Petrik, J.~Sivic, and J.~Carpentier, ``Gjk++: Leveraging acceleration methods for faster collision detection,'' \emph{IEEE Transactions on Robotics}, 2024.
\bibitem{pan2015hppfcl}
J.~Pan, S.~Chitta, D.~Manocha, F.~Lamiraux, J.~Mirabel, J.~Carpentier \emph{et~al.}, ``{HPP-FCL: An Extension of the Flexible Collision Library},'' \url{https://github.com/humanoid-path-planner/hpp-fcl}, 2015--2022.
\bibitem{oleynikova2017voxblox}
H.~Oleynikova, Z.~Taylor, M.~Fehr, R.~Siegwart, and J.~Nieto, ``Voxblox: Incremental 3d euclidean signed distance fields for on-board mav planning,'' in \emph{IEEE/RSJ Int. Conf. Intellig. Robots \& Systems}, 2017.
\bibitem{curobo_icra23}
B.~Sundaralingam, S.~K.~S. Hari, A.~Fishman, C.~Garrett, K.~Van~Wyk, V.~Blukis, A.~Millane, H.~Oleynikova, A.~Handa, F.~Ramos, N.~Ratliff, and D.~Fox, ``Curobo: Parallelized collision-free robot motion generation,'' in \emph{IEEE Int. Conf. Robotics \& Automation (ICRA)}, 2023.
\bibitem{bien2024generating}
S.~Bien, A.~Naceri, L.~Figueredo, and S.~Haddadin, ``Generating force vectors from projective truncated signed distance fields for collision avoidance and haptic feedback,'' in \emph{IEEE/RSJ IROS}, 2024.
\bibitem{staroverov2024dynamic}
A.~Staroverov, M.~Alhaddad, A.~Narendra, K.~Mironov, and A.~Panov, ``Dynamic neural potential field: Online trajectory optimization in presence of moving obstacles,'' \emph{arXiv preprint arXiv:2410.06819}, 2024.
\bibitem{eckhoff2023towards}
M.~Eckhoff, D.~Knobbe, H.~Zwirnmann, A.~Swikir, and S.~Haddadin, ``Towards connecting control to perception: High-performance whole-body collision avoidance using control-compatible obstacles,'' in \emph{IEEE/RSJ Int. Conf. Intellig. Robots \& Systems (IROS)}, 2023.
\bibitem{braquet2022vector}
M.~Braquet and E.~Bakolas, ``Vector field-based collision avoidance for moving obstacles with time-varying elliptical shape,'' \emph{IFAC}, 2022.
\bibitem{radhakrishnan2024state}
S.~Radhakrishnan and W.~Gueaieb, ``A state-of-the-art review on topology and differential geometry-based robotic path planning—part ii: planning under dynamic constraints,'' \emph{IJIRA}, vol.~8, no.~2, pp. 455--479, 2024.
\bibitem{laha2022coordinate}
R.~Laha, R.~Sun, W.~Wu, D.~Mahalingam, N.~Chakraborty, L.~Figueredo, and S.~Haddadin, ``Coordinate inv. user-guided constrained planning w/ reactive rapidly expanding plane-oriented escaping trees,'' in \emph{ICRA}, 2022.
\bibitem{laha2021reactive}
R.~Laha, L.~F. Figueredo, J.~Vrabel, A.~Swikir, and S.~Haddadin, ``Reactive cooperative manipulation based on set primitives and circular fields,'' in \emph{IEEE ICRA}, 2021.
\end{thebibliography}