We present a sampling-based model predictive control method that uses a generic physics simulator as the dynamical model. In particular, we propose a Model Predictive Path Integral controller (MPPI) that employs the GPU-parallelizable IsaacGym simulator to compute the forward dynamics of the robot and environment. Since the simulator implicitly defines the dynamic model, our method is readily extendable to different objects and robots, allowing one to solve complex navigation and contact-rich tasks. We demonstrate the effectiveness of this method in several simulated and real-world settings, including mobile navigation with collision avoidance, non-prehensile manipulation, and whole-body control for high-dimensional configuration spaces. This is a powerful and accessible open-source tool to solve many contact-rich motion planning tasks.

Representing the 3D environment with instance-aware semantic and geometric information is crucial for interaction-aware robots in dynamic environments. Nevertheless, creating such a representation poses challenges due to sensor noise, instance segmentation and tracking errors, and the objects' dynamic motion. This paper introduces a novel particle-based instance-aware semantic occupancy map to tackle these challenges. Particles with an augmented instance state are used to estimate the Probability Hypothesis Density (PHD) of the objects and implicitly model the environment. Utilizing a State-augmented Sequential Monte Carlo PHD (S² MC-PHD) filter, these particles are updated to jointly estimate occupancy status, semantic, and instance IDs, mitigating noise. Additionally, a memory module is adopted to enhance the map's responsiveness to previously observed objects. Experimental results on the Virtual KITTI 2 dataset demonstrate that the proposed approach surpasses state-of-the-art methods across multiple metrics under different noise conditions. Subsequent tests using real-world data further validate the effectiveness of the proposed approach.

Robots will increasingly operate near humans that introduce uncertainties in the motion planning problem due to their complex nature. Optimization-based planners typically avoid humans through collision avoidance chance constraints. This allows the planner to optimize performance while guaranteeing probabilistic safety. However, existing real-time methods do not consider the actual probability of collision for the planned trajectory but rather its marginalization, that is, the independent collision probabilities for each planning step and/or dynamic obstacle, resulting in conservative trajectories. To address this issue, we introduce a novel real-time capable method termed Safe Horizon MPC that explicitly constrains the joint probability of collision with all obstacles over the duration of the motion plan. This is achieved by reformulating the chance-constrained planning problem using scenario optimization and predictive control. Out of sampled realizations of human motion, we identify which cases affect the optimization. This allows us to certify the planned trajectory in real-time. Our method is less conservative than state-of-the-art approaches, applicable to arbitrary probability distributions of the obstacles’ trajectories, computationally tractable and scalable. We demonstrate our proposed approach using a mobile robot and an autonomous vehicle in an environment shared with humans.

This study investigates the impact of walking and e-hailing on the scale economies of on-demand mobility services. An analytical framework is developed to i) explicitly characterize the physical interactions between passengers and vehicles in the matching and pickup processes, and ii) derive the closed-form degree of scale economies (DSE) to quantify scale economies. The general model is then specified for conventional street-hailing and e-hailing, with and without walking before pickup and after dropoff. We show that, under a system-optimum fleet size, the market always exhibits economies of scale regardless of the matching mechanism and the walking behaviors, though the scale effect diminishes as passenger demand increases. Yet, street-hailing and e-hailing show different scale economies in their matching process. While street-hailing matching shows a constant DSE of two, e-hailing matching is more sensitive to demand and its DSE diminishes to one when passenger competition emerges. Walking, on the other hand, has mixed effects on the scale economies: while the reduced pickup and in-vehicle times bring a positive scale effect, the extra walking time and possible concentration of vacant vehicles and waiting passengers on streets negatively affect scale economies. All these analytical results are validated through agent-based simulations on Manhattan with real-life demand patterns.

Dynamic obstacle avoidance is a popular research topic for autonomous systems, such as micro aerial vehicles and service robots. Accurately evaluating the performance of dynamic obstacle avoidance methods necessitates the establishment of a metric to quantify the environment's difficulty, a crucial aspect that remains unexplored. In this paper, we propose four metrics to measure the difficulty of dynamic environments. These metrics aim to comprehensively capture the influence of obstacles' number, size, velocity, and other factors on the difficulty. We compare the proposed metrics with existing static environment difficulty metrics and validate them through over 1.5 million trials in a customized simulator. This simulator excludes the effects of perception and control errors and supports different motion and gaze planners for obstacle avoidance. The results indicate that the survivability metric outperforms and establishes a monotonic relationship between the success rate, with a Spearman's Rank Correlation Coefficient (SRCC) of over 0.9. Specifically, for every planner, lower survivability leads to a higher success rate. This metric not only facilitates fair and comprehensive benchmarking but also provides insights for refining collision avoidance methods, thereby furthering the evolution of autonomous systems in dynamic environments.

This work formally defines the problem of fleet sizing with delays (FSD), where the option of delaying individual tasks within fleet sizing is considered. We prove that the FSD problem is NP-hard and solve a formulation of the FSD problem as a mixed integer linear problem (MILP). We then analyze the proposed method in detail in an abstract case and validate it in a case study of taxi rides in Manhattan. We show that fleet sizes can be decreased significantly and that the trade-off space of the number of required vehicles to execution time and added delay can be enlarged.

Motion planning for autonomous robots in dynamic environments poses numerous challenges due to uncertainties in the robot's dynamics and interaction with other agents. Sampling-based MPC approaches, such as Model Predictive Path Integral (MPPI) control, have shown promise in addressing these complex motion planning problems. However, the performance of MPPI relies heavily on the choice of sampling distribution. Existing literature often uses the previously computed input sequence as the mean of a Gaussian distribution for sampling, leading to potential failures and local minima. We propose a novel derivation of MPPI that allows for arbitrary sampling distributions to enhance efficiency, robustness, and convergence while alleviating the problem of local minima. We present an efficient importance sampling scheme that combines classical and learning-based ancillary controllers simultaneously, resulting in more informative sampling and control fusion. Several simulated and real-world demonstrate the validity of our approach.

Integrating on-demand services into public transport networks might be the best way to face the current situation in which these new technologies have increased congestion in most cities. When cooperating with on-demand services rather than competing with them, public transport would not risk losing users, and could attract some passengers from private modes thanks to an increased quality of service. This fact has engendered a growing literature discussing how to design such an integrated system. However, all of that research has imposed that on-demand mobility is to solve the so-called “last-mile problem”, serving only as a feeder that connects the exact origins/destinations with the traditional public transit network. As it induces a large number of transfers and it precludes some scale-effects to be triggered, in this paper we challenge that imposition and investigate if this is the best spatial integration strategy. To do so, we study a simplified linear city in a morning peak situation, where we propose seven different line structures, all of them combining a traditional fixed line with on-demand ride-pooling (ODRP): three direct structures, where ODRP can serve full trips, three semi-direct, where a single ODRP vehicle can serve the largest part of a trip, and a base case in which ODRP is restricted to the first and final legs only. Our results show that the base case is optimal only under very specific demand patterns, or when transfer penalties are disregarded. Our analytical approach reveals relevant operational aspects of such integrated systems: namely, that the base case can help increase directness (diminishing detours), and that ODRP can help shorten the routes of the fixed services to decrease operator costs.

Autonomous mobile robots require predictions of human motion to plan a safe trajectory that avoids them. Because human motion cannot be predicted exactly, future trajectories are typically inferred from real-world data via learning-based approximations. These approximations provide useful information on the pedestrian's behavior, but may deviate from the data, which can lead to collisions during planning. In this work, we introduce a joint prediction and planning framework, Partitioned Scenario Replay (PSR), that stores and partitions previously observed human trajectories, referred to as scenarios. During planning, scenarios observed in similar situations are reintroduced (or replayed) as motion predictions. By sampling real data and by building on scenario optimization and predictive control, the planner provides probabilistic collision avoidance guarantees in the real-world. Relying on this guarantee to remain safe, PSR can incrementally improve its prediction and planning performance online. We demonstrate our approach on a mobile robot navigating around pedestrians.

This paper proposes a decentralized trajectory planning framework for the collision avoidance problem of multiple micro aerial vehicles (MAVs) in environments with static and dynamic obstacles. The framework utilizes spatiotemporal occupancy grid maps (SOGM), which forecast the occupancy status of neighboring space in the near future, as the environment representation. Based on this representation, we extend the kinodynamic A∗ and the corridor-constrained trajectory optimization algorithms to efficiently tackle static and dynamic obstacles with arbitrary shapes. Collision avoidance between communicating robots is integrated by sharing planned trajectories and projecting them onto the SOGM. The simulation results show that our method achieves competitive performance against state-of-the-art methods in dynamic environments with different numbers and shapes of obstacles. Finally, the proposed method is validated in real experiments.

Ground robots navigating in complex, dynamic environments must compute collision-free trajectories to avoid obstacles safely and efficiently. Nonconvex optimization is a popular method to compute a trajectory in real-time. However, these methods often converge to locally optimal solutions and frequently switch between different local minima, leading to inefficient and unsafe robot motion. In this work, we propose a novel topology-driven trajectory optimization strategy for dynamic environments that plans multiple distinct evasive trajectories to enhance the robot's behavior and efficiency. A global planner iteratively generates trajectories in distinct homotopy classes. These trajectories are then optimized by local planners working in parallel. While each planner shares the same navigation objectives, they are locally constrained to a specific homotopy class, meaning each local planner attempts a different evasive maneuver. The robot then executes the feasible trajectory with the lowest cost in a receding horizon manner. We demonstrate, on a mobile robot navigating among pedestrians, that our approach leads to faster trajectories than existing planners.

Contingency planning, wherein an agent generates a set of possible plans conditioned on the outcome of an uncertain event, is an increasingly popular way for robots to act under uncertainty. In this work we take a game-theoretic perspective on contingency planning, tailored to multi-agent scenarios in which a robot's actions impact the decisions of other agents and vice versa. The resulting contingency game allows the robot to efficiently interact with other agents by generating strategic motion plans conditioned on multiple possible intents for other actors in the scene. Contingency games are parameterized via a scalar variable which represents a future time when intent uncertainty will be resolved. By estimating this parameter online, we construct a game-theoretic motion planner that adapts to changing beliefs while anticipating future certainty. We show that existing variants of game-theoretic planning under uncertainty are readily obtained as special cases of contingency games. Through a series of simulated autonomous driving scenarios, we demonstrate that contingency games close the gap between certainty-equivalent games that commit to a single hypothesis and non-contingent multi-hypothesis games that do not account for future uncertainty reduction.

As robots shift from industrial to human-centered spaces, adopting mobile manipulators, which expand workspace capabilities, becomes crucial. In these settings, seamless interaction with humans necessitates compliant control. Two common methods for safe interaction, admittance, and impedance control, require force or torque sensors, often absent in lower-cost or lightweight robots. This paper presents an adaption of impedance control that can be used on current-controlled robots without the use of force or torque sensors and shows its application for compliant control of a mobile manipulator. A calibration method is designed that enables estimation of the actuators' current/torque ratios and frictions, used by the adapted impedance controller, and that can handle model errors. The calibration method and the performance of the designed controller are experimentally validated using the Kinova GEN3 Lite arm. Results show that the calibration method is consistent and that the designed controller for the arm is compliant while also being able to track targets with five-millimeter precision when no interaction is present. Additionally, this paper presents two operational modes for interacting with the mobile manipulator: one for guiding the robot around the workspace through interacting with the arm and another for executing a tracking task, both maintaining compliance to external forces. These operational modes were tested in real-world experiments, affirming their practical applicability and effectiveness.Code: https://github.com/tud-amr/mobile-manipulator-compliance

Task and Motion Planning (TAMP) has made strides in complex manipulation tasks, yet the execution robustness of the planned solutions remains overlooked. In this work, we propose a method for reactive TAMP to cope with runtime uncertainties and disturbances. We combine an Active Inference planner (AIP) for adaptive high-level action selection and a novel Multi-Modal Model Predictive Path Integral controller (M3P2I) for low-level control. This results in a scheme that simultaneously adapts both high-level actions and low-level motions. The AIP generates alternative symbolic plans, each linked to a cost function for M3P2I. The latter employs a physics simulator for diverse trajectory rollouts, deriving optimal control by weighing the different samples according to their cost. This idea enables blending different robot skills for fluid and reactive plan execution, accommodating plan adjustments at both the high and low levels to cope, for instance, with dynamic obstacles or disturbances that invalidate the current plan. We have tested our approach in simulations and real-world scenarios.

Risk assessment is a crucial component of collision warning and avoidance systems for intelligent vehicles. Reachability-based formal approaches have been developed to ensure driving safety to accurately detect potential vehicle collisions. However, they suffer from over-conservatism, potentially resulting in false–positive risk events in complicated real-world applications. In this paper, we combine two reachability analysis techniques, a backward reachable set (BRS) and a stochastic forward reachable set (FRS), and propose an integrated probabilistic collision–detection framework for highway driving. Within this framework, we can first use a BRS to formally check whether a two-vehicle interaction is safe; otherwise, a prediction-based stochastic FRS is employed to estimate the collision probability at each future time step. Thus, the framework can not only identify non-risky events with guaranteed safety but also provide accurate collision risk estimation in safety–critical events. To construct the stochastic FRS, we develop a neural network-based acceleration model for surrounding vehicles and further incorporate a confidence-aware dynamic belief to improve the prediction accuracy. Extensive experiments were conducted to validate the performance of the acceleration prediction model based on naturalistic highway driving data. The efficiency and effectiveness of the framework with infused confidence beliefs were tested in both naturalistic and simulated highway scenarios. The proposed risk assessment framework is promising for real-world applications.

Many problems in robotics seek to simultaneously optimize several competing objectives. A conventional approach is to create a single cost function comprised of the weighted sum of the individual objectives. Solutions to this scalarized optimization problem are Pareto optimal solutions to the original multi-objective problem. However, finding an accurate representation of a Pareto front remains an important challenge. Uniformly spaced weights are often inefficient and do not provide error bounds. We address the problem of computing a finite set of weights whose optimal solutions closely approximate the solution of any other weight vector. To this end, we prove fundamental properties of the optimal cost as a function of the weight vector. We propose an algorithm that greedily adds the weight vector least-represented by the current set, and provide bounds on the regret. We extend our method to include suboptimal solvers for the scalarized optimization, and handle stochastic inputs to the planning problem. Finally, we illustrate that the proposed approach significantly outperforms baseline approaches for different robot planning problems with varying numbers of objective functions.

On-demand ridepooling (ODRP) vehicles follow routes that are fully flexible. However, when the system does not provide door-to-door service and users can be asked to walk, their paths tend to concentrate, particularly along main streets that connect highly demanded areas of the city. These frequently travelled segments are hence useful to multiple passengers, which can be used as an indicator that it would be efficient to allocate a fixed public transport line there. In this paper, we formalise this idea and propose a novel method to design a public transport network, where bus fixed lines are combined with ODRP. Given a network and a transport demand, we first simulate how to serve it using only ODRP with walking. For this, we employ a state-of-the-art assignment algorithm, and take as output the resulting users’ paths. These paths are then processed by a tailored algorithm to create fixed lines where the paths accumulate the most. Users who do not have an available fixed line (i.e., those whose paths were barely shared) are served by ODRP in the mixed system. Simulations using real-life data from Utrecht, The Netherlands, and the Sunshine Coast, Australia, reveal the merits of our method compared to several benchmarks. Crucially, our method builds a small number of fixed lines while still serving the majority of the demand through them. This study contributes not only to the design of public transport networks, but also to the understanding of the patterns that naturally appear in intrinsically flexible mobility systems.