Wilson Jallet;Antoine Bambade;Etienne Arlaud;Sarah El-Kazdadi;Nicolas Mansard;Justin Carpentier
{"title":"ProxDDP: Proximal Constrained Trajectory Optimization","authors":"Wilson Jallet;Antoine Bambade;Etienne Arlaud;Sarah El-Kazdadi;Nicolas Mansard;Justin Carpentier","doi":"10.1109/TRO.2025.3554437","DOIUrl":"10.1109/TRO.2025.3554437","url":null,"abstract":"Trajectory optimization has been a popular choice for motion generation and control in robotics for at least a decade. Several numerical approaches have exhibited the required speed to enable online computation of trajectories for real-time of various systems, including complex robots. Many of these said are based on the differential dynamic programming (DDP) algorithm—initially designed for unconstrained trajectory optimization problems—and its variants, which are relatively easy to implement and provide good runtime performance. However, several problems in robot control call for using constrained formulations (e.g., torque limits, obstacle avoidance), from which several difficulties arise when trying to adapt DDP-type methods: numerical stability, computational efficiency, and constraint satisfaction. In this article, we leverage proximal methods for constrained optimization and introduce a DDP-type method for fast, constrained trajectory optimization suited for model-predictive control (MPC) applications with easy warm-starting. Compared to earlier solvers, our approach effectively manages hard constraints without warm-start limitations and exhibits good convergence behavior. We provide a complete implementation as part of an open-source and flexible C++ trajectory optimization library called <sc>aligator</small>. These algorithmic contributions are validated through several trajectory planning scenarios from the robotics literature and the real-time whole-body MPC of a quadruped robot.","PeriodicalId":50388,"journal":{"name":"IEEE Transactions on Robotics","volume":"41 ","pages":"2605-2624"},"PeriodicalIF":9.4,"publicationDate":"2025-03-24","publicationTypes":"Journal Article","fieldsOfStudy":null,"isOpenAccess":false,"openAccessPdf":"","citationCount":null,"resultStr":null,"platform":"Semanticscholar","paperid":"143702799","PeriodicalName":null,"FirstCategoryId":null,"ListUrlMain":null,"RegionNum":1,"RegionCategory":"计算机科学","ArticlePicture":[],"TitleCN":null,"AbstractTextCN":null,"PMCID":"","EPubDate":null,"PubModel":null,"JCR":null,"JCRName":null,"Score":null,"Total":0}
Mengchao Zhang;Devesh K. Jha;Arvind U. Raghunathan;Kris Hauser
{"title":"Simultaneous Trajectory Optimization and Contact Selection for Contact-Rich Manipulation With High-Fidelity Geometry","authors":"Mengchao Zhang;Devesh K. Jha;Arvind U. Raghunathan;Kris Hauser","doi":"10.1109/TRO.2025.3554380","DOIUrl":"10.1109/TRO.2025.3554380","url":null,"abstract":"Contact-implicit trajectory optimization (CITO) is an effective method to plan complex trajectories for various contact-rich systems including manipulation and locomotion. CITO formulates a mathematical program with complementarity constraints (MPCC) that enforces that contact forces must be zero when points are not in contact. However, MPCC solve times increase steeply with the number of allowable points of contact, which limits CITO's applicability to problems in which only a few, simple geometries are allowed us to make contact. This article introduces simultaneous trajectory optimization and contact selection (STOCS), as an extension of CITO that overcomes this limitation. The innovation of STOCS is to identify salient contact points and times inside the iterative trajectory optimization process. This effectively reduces the number of variables and constraints in each MPCC invocation. The STOCS framework, instantiated with key contact identification subroutines, renders the optimization of manipulation trajectories computationally tractable even for high-fidelity geometries consisting of tens of thousands of vertices.","PeriodicalId":50388,"journal":{"name":"IEEE Transactions on Robotics","volume":"41 ","pages":"2677-2690"},"PeriodicalIF":9.4,"publicationDate":"2025-03-24","publicationTypes":"Journal Article","fieldsOfStudy":null,"isOpenAccess":false,"openAccessPdf":"","citationCount":null,"resultStr":null,"platform":"Semanticscholar","paperid":"143702800","PeriodicalName":null,"FirstCategoryId":null,"ListUrlMain":null,"RegionNum":1,"RegionCategory":"计算机科学","ArticlePicture":[],"TitleCN":null,"AbstractTextCN":null,"PMCID":"","EPubDate":null,"PubModel":null,"JCR":null,"JCRName":null,"Score":null,"Total":0}
{"title":"AQUA-SLAM: Tightly Coupled Underwater Acoustic-Visual-Inertial SLAM With Sensor Calibration","authors":"Shida Xu;Kaicheng Zhang;Sen Wang","doi":"10.1109/TRO.2025.3554396","DOIUrl":"10.1109/TRO.2025.3554396","url":null,"abstract":"Underwater environments pose significant challenges for visual simultaneous localization and mapping (SLAM) systems due to limited visibility, inadequate illumination, and sporadic loss of structural features in images. Addressing these challenges, this article introduces a novel, tightly coupled acoustic-visual-inertial SLAM approach, termed AQUA-SLAM, to fuse a Doppler velocity log (DVL), a stereo camera, and an inertial measurement unit (IMU) within a graph optimization framework. Moreover, we propose an efficient sensor calibration technique, encompassing the multisensor extrinsic calibration (among the DVL, camera, and IMU) and the DVL transducer misalignment calibration, with a fast linear approximation procedure for real-time online execution. The proposed methods are extensively evaluated in a tank environment with ground truth, and validated for offshore applications in the North Sea. The results demonstrate that our method surpasses current state-of-the-art underwater and visual-inertial SLAM systems in terms of localization accuracy and robustness. The proposed system will be made open-source for the community.","PeriodicalId":50388,"journal":{"name":"IEEE Transactions on Robotics","volume":"41 ","pages":"2785-2803"},"PeriodicalIF":9.4,"publicationDate":"2025-03-24","publicationTypes":"Journal Article","fieldsOfStudy":null,"isOpenAccess":false,"openAccessPdf":"","citationCount":null,"resultStr":null,"platform":"Semanticscholar","paperid":"143702798","PeriodicalName":null,"FirstCategoryId":null,"ListUrlMain":null,"RegionNum":1,"RegionCategory":"计算机科学","ArticlePicture":[],"TitleCN":null,"AbstractTextCN":null,"PMCID":"","EPubDate":null,"PubModel":null,"JCR":null,"JCRName":null,"Score":null,"Total":0}
{"title":"Linearized Virtual Energy Tank for Passivity-Based Bilateral Teleoperation Using Linear MPC","authors":"Nicola Piccinelli;Riccardo Muradore","doi":"10.1109/TRO.2025.3554447","DOIUrl":"10.1109/TRO.2025.3554447","url":null,"abstract":"Bilateral teleoperation systems are often used in safety–critical scenarios where human operators may interact with the environment remotely, as in robotic-assisted surgery or nuclear plant maintenance. Teleoperation's stability and transparency are the two most important properties to be satisfied, but they cannot be optimized independently since they are in contrast. This article presents a passive linear MPC control scheme to implement bilateral teleoperation that optimizes the tradeoff between stability and transparency (a.k.a. performance). First, we introduce a linear virtual energy tank with a novel energy-sharing policy, allowing us to define a passive linear model predictive control (MPC). Second, we provide conditions to guarantee the stability of the nonlinear closed-loop system. We validate the proposed approach in a teleoperation scheme using two 7-degree of freedom manipulators while performing an assembly task. This novel passivity-based bilateral teleoperation using linear MPC and linearized energy tank reduces the computational effort of existing passive nonlinear MPC controllers.","PeriodicalId":50388,"journal":{"name":"IEEE Transactions on Robotics","volume":"41 ","pages":"2589-2604"},"PeriodicalIF":9.4,"publicationDate":"2025-03-24","publicationTypes":"Journal Article","fieldsOfStudy":null,"isOpenAccess":false,"openAccessPdf":"","citationCount":null,"resultStr":null,"platform":"Semanticscholar","paperid":"143702803","PeriodicalName":null,"FirstCategoryId":null,"ListUrlMain":null,"RegionNum":1,"RegionCategory":"计算机科学","ArticlePicture":[],"TitleCN":null,"AbstractTextCN":null,"PMCID":"","EPubDate":null,"PubModel":null,"JCR":null,"JCRName":null,"Score":null,"Total":0}
James P. Wilson;Shalabh Gupta;Thomas A. Wettergren
{"title":"Generalized Multispeed Dubins Motion Model","authors":"James P. Wilson;Shalabh Gupta;Thomas A. Wettergren","doi":"10.1109/TRO.2025.3554436","DOIUrl":"10.1109/TRO.2025.3554436","url":null,"abstract":"The article develops a novel motion model, called generalized multispeed Dubins motion model (GMDM), which extends the Dubins model by considering multiple speeds. While the Dubins model produces time-optimal paths under a constant-speed constraint, these paths could be suboptimal if this constraint is relaxed to include multiple speeds. This is because a constant speed results in a large minimum turning radius, thus producing paths with longer maneuvers and larger travel times. In contrast, multispeed relaxation allows for slower speed sharp turns, thus producing more direct paths with shorter maneuvers and smaller travel times. Furthermore, the inability of the Dubins model to reduce speed could result in fast maneuvers near obstacles, thus producing paths with high collision risks. In this regard, GMDM provides the motion planners the ability to jointly optimize time and risk by allowing the change of speed along the path. GMDM is built upon the six Dubins path types considering the change of speed on path segments. It is theoretically established that GMDM provides full reachability of the configuration space for any speed selections. Furthermore, it is shown that the Dubins model is a specific case of GMDM for constant speeds. The solutions of GMDM are analytical and suitable for real-time applications. The performance of GMDM in terms of solution quality (i.e., time/time-risk cost) and computation time is comparatively evaluated against the existing motion models in obstacle-free as well as obstacle-rich environments via extensive Monte Carlo simulations. The results show that in obstacle-free environments, GMDM produces near time-optimal paths with significantly lower travel times than the Dubins model while having similar computation times. In obstacle-rich environments, GMDM produces time-risk optimized paths with substantially lower collision risks.","PeriodicalId":50388,"journal":{"name":"IEEE Transactions on Robotics","volume":"41 ","pages":"2861-2878"},"PeriodicalIF":9.4,"publicationDate":"2025-03-24","publicationTypes":"Journal Article","fieldsOfStudy":null,"isOpenAccess":false,"openAccessPdf":"","citationCount":null,"resultStr":null,"platform":"Semanticscholar","paperid":"143702804","PeriodicalName":null,"FirstCategoryId":null,"ListUrlMain":null,"RegionNum":1,"RegionCategory":"计算机科学","ArticlePicture":[],"TitleCN":null,"AbstractTextCN":null,"PMCID":"","EPubDate":null,"PubModel":null,"JCR":null,"JCRName":null,"Score":null,"Total":0}
{"title":"Accelerated Reeds–Shepp and Underspecified Reeds–Shepp Algorithms for Mobile Robot Path Planning","authors":"Ibrahim Ibrahim;Wilm Decré;Jan Swevers","doi":"10.1109/TRO.2025.3554406","DOIUrl":"10.1109/TRO.2025.3554406","url":null,"abstract":"In this study, we present a simple and intuitive method for accelerating optimal Reeds–Shepp path computation. Our approach uses geometrical reasoning to analyze the behavior of optimal paths, resulting in a new partitioning of the state space and a further reduction in the minimal set of viable paths. We revisit and reimplement classic methodologies from literature, which lack contemporary open-source implementations, to serve as benchmarks for evaluating our method. In addition, we address the underspecified Reeds–Shepp planning problem where the final orientation is unspecified. We perform exhaustive experiments to validate our solutions. Compared to the modern C++ implementation of the original Reeds–Shepp solution in the Open Motion Planning Library, our method demonstrates a <inline-formula><tex-math>$15times$</tex-math></inline-formula> speedup, while classic methods achieve a <inline-formula><tex-math>$5.79times$</tex-math></inline-formula> speedup. Both approaches exhibit machine-precision differences in path lengths compared to the original solution. We release our proposed C++ implementations for both the accelerated and underspecified Reeds–Shepp problems as open-source code.","PeriodicalId":50388,"journal":{"name":"IEEE Transactions on Robotics","volume":"41 ","pages":"2691-2708"},"PeriodicalIF":9.4,"publicationDate":"2025-03-24","publicationTypes":"Journal Article","fieldsOfStudy":null,"isOpenAccess":false,"openAccessPdf":"","citationCount":null,"resultStr":null,"platform":"Semanticscholar","paperid":"143702797","PeriodicalName":null,"FirstCategoryId":null,"ListUrlMain":null,"RegionNum":1,"RegionCategory":"计算机科学","ArticlePicture":[],"TitleCN":null,"AbstractTextCN":null,"PMCID":"","EPubDate":null,"PubModel":null,"JCR":null,"JCRName":null,"Score":null,"Total":0}
{"title":"SLIM: Scalable and Lightweight LiDAR Mapping in Urban Environments","authors":"Zehuan Yu;Zhijian Qiao;Wenyi Liu;Huan Yin;Shaojie Shen","doi":"10.1109/TRO.2025.3554400","DOIUrl":"10.1109/TRO.2025.3554400","url":null,"abstract":"Light detection and ranging (LiDAR) point cloud maps are extensively utilized on roads for robot navigation due to their high consistency. However, dense point clouds face challenges of high memory consumption and reduced maintainability for long-term operations. In this study, we introduce scalable and lightweight LiDAR mapping (SLIM), a scalable and lightweight mapping system for long-term LiDAR mapping in urban environments. The system begins by parameterizing structural point clouds into lines and planes. These lightweight and structural representations meet the requirements of map merging, pose graph optimization, and bundle adjustment, ensuring incremental management and local consistency. For long-term operations, a map-centric nonlinear factor recovery method is designed to sparsify poses while preserving mapping accuracy. We validate the SLIM system with multisession real-world LiDAR data from classical LiDAR mapping datasets, including KITTI, NCLT, HeLiPR, and M2DGR. The experiments demonstrate its capabilities in mapping accuracy, lightweightness, and scalability. Map reuse is also verified through map-based robot localization. Finally, with multisession LiDAR data, the SLIM system provides a globally consistent map with low memory consumption (<inline-formula><tex-math>$sim$</tex-math></inline-formula> 130 KB/km on KITTI).","PeriodicalId":50388,"journal":{"name":"IEEE Transactions on Robotics","volume":"41 ","pages":"2569-2588"},"PeriodicalIF":9.4,"publicationDate":"2025-03-24","publicationTypes":"Journal Article","fieldsOfStudy":null,"isOpenAccess":false,"openAccessPdf":"","citationCount":null,"resultStr":null,"platform":"Semanticscholar","paperid":"143702801","PeriodicalName":null,"FirstCategoryId":null,"ListUrlMain":null,"RegionNum":1,"RegionCategory":"计算机科学","ArticlePicture":[],"TitleCN":null,"AbstractTextCN":null,"PMCID":"","EPubDate":null,"PubModel":null,"JCR":null,"JCRName":null,"Score":null,"Total":0}
Fouad Sukkar;Jennifer Wakulicz;Ki Myung Brian Lee;Weiming Zhi;Robert Fitch
{"title":"Multiquery Robotic Manipulator Task Sequencing With Gromov-Hausdorff Approximations","authors":"Fouad Sukkar;Jennifer Wakulicz;Ki Myung Brian Lee;Weiming Zhi;Robert Fitch","doi":"10.1109/TRO.2025.3554404","DOIUrl":"10.1109/TRO.2025.3554404","url":null,"abstract":"Robotic manipulator applications often require efficient online motion planning. When completing multiple tasks, sequence order and choice of goal configuration can have a drastic impact on planning performance. This is well known as the robot task sequencing problem (RTSP). Existing general-purpose RTSP algorithms are susceptible to producing poor-quality solutions or failing entirely when available computation time is restricted. We propose a new multiquery task sequencing method designed to operate in semistructured environments with a combination of static and nonstatic obstacles. Our method intentionally trades off workspace generality for planning efficiency. Given a user-defined task space with static obstacles, we compute a subspace decomposition. The key idea is to establish approximate isometries known as <inline-formula><tex-math>$epsilon$</tex-math></inline-formula>-Gromov-Hausdorff approximations that identify points that are close to one another in both task and configuration space. Importantly, we prove bounded suboptimality guarantees on the lengths of paths within these subspaces. These bounding relations further imply that paths within the same subspace can be smoothly concatenated, which we show is useful for determining efficient task sequences. We evaluate our method with several kinematic configurations in a complex simulated environment, achieving up to 3× faster motion planning and 5× lower maximum trajectory jerk compared to baselines.","PeriodicalId":50388,"journal":{"name":"IEEE Transactions on Robotics","volume":"41 ","pages":"2843-2860"},"PeriodicalIF":9.4,"publicationDate":"2025-03-24","publicationTypes":"Journal Article","fieldsOfStudy":null,"isOpenAccess":false,"openAccessPdf":"","citationCount":null,"resultStr":null,"platform":"Semanticscholar","paperid":"143702796","PeriodicalName":null,"FirstCategoryId":null,"ListUrlMain":null,"RegionNum":1,"RegionCategory":"计算机科学","ArticlePicture":[],"TitleCN":null,"AbstractTextCN":null,"PMCID":"","EPubDate":null,"PubModel":null,"JCR":null,"JCRName":null,"Score":null,"Total":0}
{"title":"Sensitivity-Aware Model Predictive Control for Robots With Parametric Uncertainty","authors":"Tommaso Belvedere;Marco Cognetti;Giuseppe Oriolo;Paolo Robuffo Giordano","doi":"10.1109/TRO.2025.3554415","DOIUrl":"10.1109/TRO.2025.3554415","url":null,"abstract":"This article introduces a computationally efficient robust model predictive control (MPC) scheme for controlling nonlinear systems affected by parametric uncertainties in their models. The approach leverages the recent notion of <italic>closed-loop state sensitivity</i> and the associated ellipsoidal tubes of perturbed trajectories for taking into account online time-varying restrictions on state and input constraints. This makes the MPC controller “aware” of potential additional requirements needed to cope with parametric uncertainty, thus significantly improving the tracking performance and success rates during navigation in constrained environments. One key contribution lies in the introduction of a computationally efficient robust MPC formulation with a <italic>comparable computational complexity</i> to a standard MPC (i.e., an MPC not explicitly dealing with parametric uncertainty). An extensive simulation campaign is presented to demonstrate the effectiveness of the proposed approach in handling parametric uncertainties and enhancing task performance, safety, and overall robustness. Furthermore, we also provide an experimental validation that shows the feasibility of the approach in real-world conditions and corroborates the statistical findings of the simulation campaign. The versatility and efficiency of the proposed method make it therefore a valuable tool for real-time control of robots subject to nonnegligible uncertainty in their models.","PeriodicalId":50388,"journal":{"name":"IEEE Transactions on Robotics","volume":"41 ","pages":"3039-3058"},"PeriodicalIF":9.4,"publicationDate":"2025-03-24","publicationTypes":"Journal Article","fieldsOfStudy":null,"isOpenAccess":false,"openAccessPdf":"","citationCount":null,"resultStr":null,"platform":"Semanticscholar","paperid":"143702802","PeriodicalName":null,"FirstCategoryId":null,"ListUrlMain":null,"RegionNum":1,"RegionCategory":"计算机科学","ArticlePicture":[],"TitleCN":null,"AbstractTextCN":null,"PMCID":"","EPubDate":null,"PubModel":null,"JCR":null,"JCRName":null,"Score":null,"Total":0}
{"title":"A Wearable Isokinetic Training Robot for Enhanced Bedside Knee Rehabilitation","authors":"Yanggang Feng;Xingyu Hu;Yuebing Li;Ke Ma;Jiaxin Ren;Zhihao Zhou;Fuzhen Yuan;Yan Huang;Liu Wang;Qining Wang;Wuxiang Zhang;Xilun Ding","doi":"10.1109/TRO.2025.3552332","DOIUrl":"10.1109/TRO.2025.3552332","url":null,"abstract":"Knee pain is prevalent in over 20% of the population, limiting the mobility of those affected. In turn, isokinetic dynamometers and robots have been used to facilitate rehabilitation for those still capable of ambulation. However, there are at most only a few wearable robots capable of delivering isokinetic training for bedridden patients. Here, we developed a wearable robot that provides bedside isokinetic training by utilizing a variable stiffness actuator and dynamic energy regeneration. The efficacy of this device was validated in a study involving six subjects with debilitating knee injuries. During two courses of rehabilitation over a total of three weeks, the average peak torque, average torque, and average work produced by their affected knees increased significantly by 81.0%, 101.4%, and 117.6%, respectively. Furthermore, the device's energy regeneration features were found capable of extending its operating time to 198 days under normal usage, representing a 57.8% increase over the same device without regeneration. These results suggest potential methodologies for delivering isokinetic joint rehabilitation to bedridden patients in areas with limited infrastructure.","PeriodicalId":50388,"journal":{"name":"IEEE Transactions on Robotics","volume":"41 ","pages":"2460-2476"},"PeriodicalIF":9.4,"publicationDate":"2025-03-18","publicationTypes":"Journal Article","fieldsOfStudy":null,"isOpenAccess":false,"openAccessPdf":"","citationCount":null,"resultStr":null,"platform":"Semanticscholar","paperid":"143661154","PeriodicalName":null,"FirstCategoryId":null,"ListUrlMain":null,"RegionNum":1,"RegionCategory":"计算机科学","ArticlePicture":[],"TitleCN":null,"AbstractTextCN":null,"PMCID":"","EPubDate":null,"PubModel":null,"JCR":null,"JCRName":null,"Score":null,"Total":0}