Path Planning for stochastic hybrid systems presents a unique challenge of predicting distributions of future states subject to a state-dependent dynamics switching function. In this work, we propose a variant of Model Predictive Path Integral Control (MPPI) to plan kinodynamic paths for such systems. Monte Carlo may be inaccurate when few samples are chosen to predict future states under state-dependent disturbances. We employ recently proposed Unscented Transform-based methods to capture stochasticity in the states as well as the state-dependent switching surfaces. This is in contrast to previous works that perform switching based only on the mean of predicted states. We focus our motion planning application on the navigation of a mobile robot in the presence of dynamically moving agents whose responses are based on sensor-constrained attention zones. We evaluate our framework on a simulated mobile robot and show faster convergence to a goal without collisions when the robot exploits the hybrid human dynamics versus when it does not.

This paper presents a novel method for modeling the shape of a continuum robot as a Neural Configuration Euclidean Distance Function (N-CEDF). By learning separate distance fields for each link and combining them through the kinematics chain, the learned N-CEDF provides an accurate and computationally efficient representation of the robot's shape. The key advantage of a distance function representation of a continuum robot is that it enables efficient collision checking for motion planning in dynamic and cluttered environments, even with point-cloud observations. We integrate the N-CEDF into a Model Predictive Path Integral (MPPI) controller to generate safe trajectories. The proposed approach is validated for continuum robots with various links in several simulated environments with static and dynamic obstacles.

The rapid advancement of robotics necessitates robust tools for developing and testing safe control architectures in dynamic and uncertain environments. Ensuring safety and reliability in robotics, especially in safety-critical applications, is crucial, driving substantial industrial and academic efforts. In this context, we extend CBFkit, a Python/ROS2 toolbox, which now incorporates a planner using reach-avoid specifications as a cost function. This integration with the Model Predictive Path Integral (MPPI) controllers enables the toolbox to satisfy complex tasks while ensuring formal safety guarantees under various sources of uncertainty using Control Barrier Functions (CBFs). CBFkit is optimized for speed using JAX for automatic differentiation and jaxopt for quadratic program solving. The toolbox supports various robotic applications, including autonomous navigation, human-robot interaction, and multi-robot coordination. The toolbox also offers a comprehensive library of planner, controller, sensor, and estimator implementations. Through a series of examples, we demonstrate the enhanced capabilities of CBFkit in different robotic scenarios.

We present a multi-rate control architecture that leverages fundamental properties of differential flatness to synthesize controllers for safety-critical nonlinear dynamical systems. We propose a two-layer architecture, where the high-level generates reference trajectories using a linear Model Predictive Controller, and the low-level tracks this reference using a feedback controller. The novelty lies in how we couple these layers, to achieve formal guarantees on recursive feasibility of the MPC problem, and safety of the nonlinear system. Furthermore, using differential flatness, we provide a constructive means to synthesize the multi-rate controller, thereby removing the need to search for suitable Lyapunov or barrier functions, or to approximately linearize/discretize nonlinear dynamics. We show the synthesized controller is a convex optimization problem, making it amenable to real-time implementations. The method is demonstrated experimentally on a ground rover and a quadruped robotic system.

Control Barrier Functions offer safety certificates by dictating controllers that enforce safety constraints. However, their response depends on the classK function that is used to restrict the rate of change of the value of the barrier function along the system trajectories. This paper introduces the notion of a Rate-Tunable (RT) CBF, which allows for online tuning of the response of CBF-based controllers. In contrast to existing approaches that use a fixed classK function to ensure safety, we parameterize and adapt the classK function parameters online. We show that this helps improve the system's response in terms of the resulting trajectories being closer to a nominal reference while being sufficiently far from the boundary of the safe set. We provide point-wise sufficient conditions to be imposed on any user-given parameter dynamics so that multiple CBF constraints continue to admit a common control input with time. Finally, we introduce RT-CBF parameter dynamics for decentralized noncooperative multi-agent systems, where a trust factor, computed based on the instantaneous ease of constraint satisfaction, is used to update parameters online for a less conservative response.

Quadratic programs (QP) subject to multiple time-dependent control barrier function (CBF) based constraints have been used to design safety-critical controllers. However, ensuring the existence of a solution at all times to the QP subject to multiple CBF constraints is non-trivial. We quantify the feasible solution space of the QP in terms of its volume. We introduce a novel feasible space volume monitoring control barrier function that promotes compatibility of barrier functions and, hence, existence of a solution at all times. We show empirically that our approach not only enhances feasibility but also exhibits reduced sensitivity to changes in the hyperparameters such as gains of nominal controller. Finally, paired with a global planner, we evaluate our controller for navigation among humans in the AWS Hospital gazebo environment. The proposed controller is demonstrated to outperform the standard CBF-QP controller in maintaining feasibility.

This paper addresses synthesizing receding-horizon controllers for nonlinear, control-affine dynamical systems under multiple incompatible hard and soft constraints. Handling incompatibility of constraints has mostly been addressed in literature by relaxing the soft constraints via slack variables. However, this may lead to trajectories that are far from the optimal solution and may compromise satisfaction of the hard constraints over time. In that regard, permanently dropping incompatible soft constraints may be beneficial for the satisfaction over time of the hard constraints (under the assumption that hard constraints are compatible with each other at initial time). To this end, motivated by approximate methods on the maximal feasible subset (maxFS) selection problem, we propose heuristics that depend on the Lagrange multipliers of the constraints. The main observation for using heuristics based on the Lagrange multipliers instead of slack variables (which is the standard approach in the related literature of finding maxFS) is that when the optimization is feasible, the Lagrange multiplier of a given constraint is non-zero, in contrast to the slack variable which is zero. This observation is particularly useful in the case of a dynamical nonlinear system where its control input is computed recursively as the optimization of a cost functional subject to the system dynamics and constraints, in the sense that the Lagrange multipliers of the constraints over a prediction horizon can indicate the constraints to be dropped so that the resulting constraints are compatible. The method is evaluated empirically in a case study with a robot navigating under multiple time and state constraints, and compared to a greedy method based on the Lagrange multiplier.

In this paper, we introduce a novel online model-based reinforcement learning algorithm that uses Unscented Transform to propagate uncertainty for the prediction of the future reward. Previous approaches either approximate the state distribution at each step of the prediction horizon with a Gaussian, or perform Monte Carlo simulations to estimate the rewards. Our method, depending on the number of sigma points employed, can propagate either mean and covariance with minimal points, or higher-order moments with more points similarly to Monte Carlo. The whole framework is implemented as a computational graph for online training. Furthermore, in order to prevent explosion in the number of sigma points when propagating through a generic state-dependent uncertainty model, we add sigma-point expansion and contraction layers to our graph, which are designed using the principle of moment matching. Finally, we propose gradient descent inspired by Sequential Quadratic Programming to update policy parameters in the presence of state constraints. We demonstrate the proposed method with two applications in simulation. The first one designs a stabilizing controller for the cart-pole problem when the dynamics is known with state-dependent uncertainty. The second example, following up on our previous work, tunes the parameters of a control barrier function-based Quadratic Programming controller for a leader-follower problem in the presence of input constraints.