Online Update of Safety Assurances Using Confidence-Based Predictions

Robots such as autonomous vehicles and assistive manipulators are increasingly operating in dynamic environments and close physical proximity to people. In such scenarios, the robot can leverage a human motion predictor to predict their future states and plan safe and efficient trajectories. However, no model is ever perfect -- when the observed human behavior deviates from the model predictions, the robot might plan unsafe maneuvers. Recent works have explored maintaining a confidence parameter in the human model to overcome this challenge, wherein the predicted human actions are tempered online based on the likelihood of the observed human action under the prediction model. This has opened up a new research challenge, i.e., \textit{how to compute the future human states online as the confidence parameter changes?} In this work, we propose a Hamilton-Jacobi (HJ) reachability-based approach to overcome this challenge. Treating the confidence parameter as a virtual state in the system, we compute a parameter-conditioned forward reachable tube (FRT) that provides the future human states as a function of the confidence parameter. Online, as the confidence parameter changes, we can simply query the corresponding FRT, and use it to update the robot plan. Computing parameter-conditioned FRT corresponds to an (offline) high-dimensional reachability problem, which we solve by leveraging recent advances in data-driven reachability analysis. Overall, our framework enables online maintenance and updates of safety assurances in human-robot interaction scenarios, even when the human prediction model is incorrect. We demonstrate our approach in several safety-critical autonomous driving scenarios, involving a state-of-the-art deep learning-based prediction model.

I. INTRODUCTION
When a robot operates in close proximity to humans, it often employs a model of the human's behavior to predict her future actions, and subsequently, her future states. Given the possible future states of the human, the robot may leverage online motion planning methods to plan around these moving obstacles, and generate real-time dynamically feasible and safe trajectories. However, when the predictions deviate from the true human behavior, the robot might confidently plan unsafe maneuvers, leading to violations of critical safety constraints.
To overcome this challenge, recent works have proposed estimating a confidence parameter in the human model based on how well the predictions align with the observed human behavior [1]. The confidence parameter is then used to dilate the possible future actions of the human, essentially predicting every possible human action when the confidence is low. Even though promising, this approach is hard to scale for many human-robot applications because an update in the confidence parameter requires recomputing the future human 1 Author is with the MAE Department at Princeton University; k.nakamura@princeton.edu.. 2 Author is with the ECE department at the University of Southern California; somilban@usc.edu. states online, which can be computationally demanding even for common, nonlinear dynamical systems.
In this work, we cast the computation of future human states as a Hamilton-Jacobi (HJ) reachability problem [2]- [4] and leverage recent advances in high-dimensional reachability analysis to quickly update human state predictions online. Specifically, in reachability analysis, the future human states can be obtained by computing the forward reachable tube (FRT) of the human -the set of all states that the human can reach starting from its current state under predicted control actions. Our key idea to update the FRT online is to compute a parameter-conditioned FRT of the human, wherein a family of the FRT is computed and a member of this family can be obtained by specifying the confidence parameter value. Thus, the FRT can be updated online with a simple query of the parameterized family, corresponding to the current confidence estimate.
The parameterized FRT can be obtained by adding the confidence parameter (and other environment factors that might change online) as "virtual states" to the human dynamics and computing the FRT for the augmented system using the standard reachability tools. This, however, results in a high-dimensional reachability problem, especially for high-capacity predictive models that leverage semantic envi-ronment factors for accurate predictions, which now become additional parameters in the model that are only known online. To overcome this challenge, we leverage DeepReach [5] -a reachability toolbox that builds upon recent advances in neural PDE solvers to compute high-dimensional reachable sets. DeepReach along with a parameterized FRT allows us to ensure safe human-robot interaction despite erroneous predictions.
To summarize, the key contributions of this work are: (1) incorporating confidence estimation in high-capacity human prediction models, e.g., models based on deep neural networks. The proposed framework allows us to exploit the predictive power of these models to plan efficient robot trajectories, yet ensure safety when the predictions cannot be trusted; (2) developing a Hamilton-Jacobi reachability framework to update the model confidence and the corresponding safety assurances online for safe human-robot interaction.
II. RELATED WORK Human Modeling and Prediction. It is a common viewpoint that humans are rational agents, that is, that humans act with intent. Prior work in the human-robot interaction literature has taken models stemming from econometrics and cognitive science to model humans as noisily-rational agents [6], [7]. A common model used in human-robot interaction domains is the Boltzmann model, which captures the notion that humans are exponentially more likely to choose actions that maximize some reward function [1], [8], [9]. However, reward functions that incorrectly specify human intent can lead to overly confident incorrect predictions. Furthermore, the reward functions used to model the human's goals often fail to capture semantic information that impacts human decision-making. Specifically, in contexts such as autonomous driving, semantic information like stop signs or crosswalks shape how humans make decisions. One way to leverage semantic information and make predictions in the continuous action space is to use a neural network-based human model. These models have enabled inference and planning around human arm motion [10], [11], navigation [12], [13], and autonomous driving [14]- [16] (see [17] for a survey). However, data-driven approaches are in general subject to incorrect predictions in scenarios not captured in the training data. In this work, our goal is to ensure safe human-robot interaction despite erroneous predictions. Safe Motion Planning. The notion of safety in the context of human-robot interaction is well studied [18], [19]. One common framework to verify safety of human-robot systems is Hamilton-Jacobi reachability, which computes reachable sets for systems with general nonlinear dynamics and bounded controls [2]- [4]. Works in [8], [20] use backward reachability to find the set of unsafe states and utilize them within a model predictive control framework to plan efficient trajectories. Although some of these works [8] track human model confidence, the safety-enforcing backward reachable set is typically computed for a fixed set of parameters. [1], [21]- [23] add flexibility by precomputing a small discrete bank of reachable sets that reflect different potential beliefs of the human model. The system switches between these reachable sets based on which one fits the robots estimate best at runtime. However in practice, the parameters that affect the model predictions (and subsequently the unsafe set) are not known a priori and must be observed online, such as semantic information in the environment and the model confidence parameter. Any precomputed bank of reachable sets will suffer from being overly conservative in such scenarios. This necessitates an online computation of the reachable set, which in general is too expensive to do in real time. In this work, we propose a method to update such reachable sets online in an effective fashion.

III. PROBLEM SETUP
We consider a robot operating in a human occupied space. We assume that the robot has full knowledge of the environment, and the robot and human states.

A. Agent Dynamics
We model each agent as a dynamical system, where we denote the robot and human states as x R ∈ R n and x H ∈ R m respectively. Their individual dynamics and controls are as follows: We also let ξ(τ ; x i , u i (·), t) denote the agent state at time τ starting at the state x i at time t and applying control u i (·) over the time horizon [t, τ ]. The robot is assumed to have some objective or task, such as reaching a goal state, that it needs to plan and execute a trajectory for. While the robot performs its task, it is imperative for it to never incur any safety violations. We denote by C the set of states the robot should avoid to ensure safety, e.g., because they imply physical collisions with the human. In this work, we will compute C via evaluating a forward reachable tube of the human. Running example: We introduce a running example for illustration throughout the paper. We consider a scenario where an autonomous car is interacting with a human-driven vehicle at a traffic intersection. We model both agents in this scenario as extended unicycles whereẋ = [ẋ,ẏ,θ,v] = [v cos θ, v sin θ, u 1 , u 2 ] . The vehicle controls are given by steering rate and acceleration. The unicycle model is widely used in the literature for modeling autonomous vehicles [15], [16]. Given a collision radius of R col = 4.5 m, we define C as the positions of the autonomous vehicle that are within a distance of R col of the human vehicle.

B. Human Prediction Model
In human-robot interaction scenarios, the robot typically maintains a model of human behavior in order to aid in the prediction of their future states. In previous works, the robot modeled the human as a utility-maximizing agent with respect to learned reward functions for given state-action pairs [1], [9], [22]. However, these models typically use discrete action sets rather than considering a continuous distribution over actions. Furthermore, these utility maximizing predictors rarely incorporate semantic information, limiting their predictive capabilities.
In this work, we are particularly interested in the settings where the human motion predictors might be high-capacity models that use semantic information about the environment as an input (e.g., the roadgraph and traffic light state in the context of autonomous driving), along with the human states (and possibly their history) to generate continuous distributions over human controls. We assume that at each time step t, the robot has a prediction for each time step over the prediction horizon [t, t + T ] in terms of multivariate Gaussian distribution over human control actions: Here, µ t:t+T and Σ t:t+T are the vectors and matrices of appropriate dimensions that represent the mean and covariance for the human control actions from time t to t + T . Such prediction output representations are quite common in the literature, especially when the model is data-driven (e.g., [15] and [16]). However, despite the benefits provided by semantic information and the use of continuous action distributions, predicted controls can differ significantly from the true actions taken by the human, especially in the scenarios that are scarce or not captured well in the training data. A robot leveraging this predictor can confidently plan unsafe motions in such situations. In this work, our goal is to ensure a safe human-robot interaction despite such predictor failures. To account for such failures, we maintain a model confidence parameter indicating our level of trust in the prediction model [1], [9]. We use this model confidence to scale the covariance matrix in (1) which dilates the predicted controls when model confidence is low. We discuss our approach in more detail in Sec. V-A. Running example: A variety of learned neural networkbased methods have been proposed to predict the future actions of a human-driven car [15], [16], [24], [25]. For our case studies, we use Trajectron++ [15], which takes the semantic road information (such as lane boundaries and roadgraph), the ego agent history, as well as that of the surrounding agents as inputs, and outputs a distribution over future human actions. Thus, the predicted mean and covariance matrices in (1) are functions of these inputs.

C. Safe Planning
Our overall goal is to plan efficient trajectories for the robot to complete its task while avoiding collisions with the human at all times based on an informed prediction of the human's future motion. We leverage the idea of the forward reachable tube (FRT) to investigate how the set of occupancy states of the human evolves over time. Mathematically, the FRT of the human is defined as: Intuitively, the FRT represents the set of all states a human can reach under some control starting from their current state x H . We limit conservative decision-making by considering a restricted set of physically feasible controls during the FRT computation, given by the human motion predictor in (1). These predictions are tempered by a model confidence parameter that expands the possible range of human control inputs when the confidence in the model's accuracy is low, and leave the original predictions in tact when model confidence is high. To tractably compute the FRT, we also limit the human control actions to a bounded set of inputs by defining a probability threshold γ on the distribution in (1). We discuss this further in Sec. V-B. Running example: For our running example, the FRT consists of all possible states of the human-driven car under the Trajectron++ predictions. Once, the FRT is computed, the potential future positions of the human can be obtained as: In other words, K is the projection of V(t) on the position states of the human. Given the potential future states of the human, the unsafe states for the robot at time t are given as: C = K R col , where denotes the Minkowski sum. C is essentially an expansion of the set K by R col .
Since we will maintain a confidence in the human model, as this confidence evolve in time, the control bounds that parameterize the FRT also change. Consequently, the set of unsafe states, C, that the robot needs to use during its planning change. Traditional methods for computing an FRT would require a re-computation of the entire FRT, which is generally a process too slow to do in real-time. On the other hand, using the worst case predictions (e.g., by considering all physically possible human actions) to generate FRTs can be overly conservative. This necessitates the need to be able to update the FRT in real-time as predictions change. We will do so by computing parameter-conditioned FRTs.

IV. BACKGROUND: HAMILTON-JACOBI (HJ) REACHABILITY
Our method for computing FRTs relies on Hamilton-Jacobi (HJ) reachability analysis. In this section, we provide a quick overview of HJ reachability.
HJ reachability is a game-theoretic method for guaranteeing performance and safety for general nonlinear dynamics in the face of bounded disturbances and controls. In the context of this work, HJ reachability is used to compute the forward reachable tube, or the set of states that the human can reach given some set of starting states. To compute the FRT for our system of interest, one must define a set of initial states L of the human 1 . Next, we define a function l(x) whose sub-zero level set yields L, i.e., L = {x : l(x) ≤ 0}. The FRT is then computed by solving the following Hamilton-Jacobi-Bellman variational inequality (HJB-VI) (2) min Here, ∇V (x, τ ) is the spatial derivative of the value function at time τ and H(x, τ, ∇V (x, τ )) is the Hamiltonian defined by (3) H(x, τ, ∇V (x, τ )) = max The solution to the HJB-VI in equation (2) is a value function V (x, τ ), whose zero sub-level set at time (t + T ) describes the set of all states that the human could have potentially occupied from t to (t + T ). This set of potential occupancies V(t) is formally written as Traditionally, these HJB-VIs are solved by discretizing the state space into a grid and updating the value function at those grid points at each time step over the time horizon [t, t + T ] [26]. However, this discretization means that the computational time scales exponentially in dimensions, making it particularly challenging to update the FRT in real-time even for moderately dimensional systems.
A different approach is to solve the HJB equation via learning-based methods. DeepReach is a recently developed neural-network-based HJ reachability solver [5]. DeepReach uses the HJB-VI itself to self-supervise the learning process of the solution. Advantageously, DeepReach does not discretize the state space and the solution time is more dependent on the problem complexity rather than the problem dimension. This enables the computation of highly parameterized reachable sets which can be used to update the reachable set online with a minimal amount of additional computation.

A. Confidence estimation and update.
We assume that at each time step, the robot is able to observe the true action taken by the humanū t−1 H . This means that, in hindsight, the robot can evaluate the likelihood of the true human action under its prediction model. If the human took a low likelihood action, this implies that the predictive model failed to capture some aspects of the human's true intent. Drawing from the literature on model confidence estimations, we define a parameter β ∈ R to describe the level of confidence in the predictive model of the human. This parameter originates from the cognitive science discipline [6], where it captures the rationality of a human being modeled as a reward-maximizing agent. High model confidence leads to the agent being exponentially more likely to take high reward actions and low model confidence transforms the agent's predicted distribution over controls to be uniform [1].
Given the continuous action space, we modify the definition of β. In this work, we define β ∈ [β low , β high ] which scales the covariance matrices Σ t:t+H in (1). β high is always set to be equal to 1, as having full confidence in the predictive model should capture the original predicted distribution over controls. β low is chosen to be a positive number less than β high based on the desired amount of spread in the human action that we want to safeguard against at low model confidence. Incorporating, the model confidence in our predictions leads to: where the covariance is scaled by β ≤ 1.
As opposed to previous work which only allowed a discrete set of β, this work allows β to take any value between β low and β high . This has advantages as discrete values allow less expressiveness in the model confidence and can lead to large differences in the distribution over predicted actions, and subsequently the FRT, as β changes.
To update β online based on the observed human actions, we use a Bayes filter. Specifically, the beliefs are initialized at t = 0 such that b 0 − (β low ) = b 0 − (β high ) = 0.5. The update rule for β is given by: where b(β) t − represents the a priori belief of the robot on the model confidence at time t, i.e., the probability of β =β at time t. b(β) t + represents the same probability a posteriori. f (ū t H ; µ t , 1 β Σ t ) refers to the Gaussian probability density function parameterized by mean µ t and covariance 1 β Σ t and evaluated at the observed actionū t H . Similarly to [1], we use the -static transition model where at each time step, the belief is sampled from b 0 with some probability and otherwise retains the belief computed by (4). This can be expressed as Given a belief of both β low and β high , the value chosen for β to augment the predicted distribution is simply a linear interpolation with weights determined by the beliefs.
After the human predictions are modified by the model confidence parameter, the ego vehicle has knowledge of a Gaussian distribution over human controls, which can be used to compute the FRT and the robot unsafe states as discussed in Sec. III-C. However, most of the available methods to solve HJI-VI including DeepReach deal with a bounded range of possible controls. We use a probability mass threshold γ to trim the control distribution. Specifically, for the ith entry, u i , the range of control is given as [µ i − δ, µ i + δ], where δ is chosen such that the probability density function integrates to γ over that range.
Ultimately, this process provides the robot a deterministic range of human control actions [u min (β), u max (β)] t:t+T that varies as both predictions and model confidence change at each time step.
We next address one of the core challenges of this work, i.e., updating the FRT online as the above control bounds change. We treat u min (β) t:t+T and u max (β)t : t + T as additional virtual states for the human with zero dynamics and compute the FRT for the augmented system dynamics. This results in a family of forward reachable sets characterized by the control bounds which can then be used to query any set of control actions online and obtain the corresponding FRT, and subsequently, the update collision set C.
Computing the FRT family requires solving a highdimensional reachability problem, since now the human control bounds are also states in the system. For this computation, we leverage DeepReach that is equipped to handle such problems. As long as the confidence-modified predictions fall within the control range used during training, updating the collision set for the human driver can be done in real-time. Running Example: The human FRT at any time depends on the current state of the human and the confidence-adjusted control bounds. Without loss of generality, we can assume that the human starts at the origin and has zero initial heading. To be able to compute the human FRT online for any values of the remaining parameters, we augment them to the human states. This leads to new system dynamics: with x being the 14D augmented state. As such, we should add control bounds for every time step as a state; however, in practice we have found that a linear interpolation between the initial and final predicted controls serves as a good approximation. Thus, u τ H ∈ [ũ τ min ,ũ τ max ] approximately, whereũ u τ max can be similarly defined. Finally, the function l(x) encoding the initial state of the robot is given by where (x, y, v, θ) represents the human-driven vehicle state as usual. 1 , 2 , 3 are small margins defined to have some mass in the initial set of states (for numerical purposes). The FRT can now be computed using the HJI-VI as described in Sec. IV. In this case, it is a 14-dimensional computation that would not be possible using traditional reachability methods (typically limited to 6D). Instead, using DeepReach we are able to learn the value function solution to the HJB-VI under these dynamics. Once learned, we can simply query the control bounds given by the current model predictions and the confidence parameter. Correspondingly, the set V(t) describes the set of positions, orientations and velocities  that the human can reach under confidence-adjusted control inputs, starting from v start . The set of unsafe robot states can then be recovered as described in Sec. III-C.

VI. CASE STUDIES
To illustrate our approach, we use the NuScenes dataset [27] to examine human-robot interaction for two traffic scenarios based on our running example. The NuScenes dataset has annotated semantic maps along with trajectory data for every 0.5 seconds. In the first case study, the humandriven vehicle begins a U-turn maneuver from the right most lane as the autonomous vehicle approaches an intersection. In the second case study, the autonomous vehicle attempts to cross a protected intersection, but the human driver fails to yield and proceeds past a stop sign too quickly.
In these case studies, we use Trajectron++, a state-ofthe-art human motion predictor, which utilizes the semantic information provided by NuScenes to better inform the distribution over future human actions. The confidence in Trajectron++ at each timestep further parameterizes these distributions in the predictive model.
For these case studies, the parameters were set such that β low = 0.03 and γ = 0.07. The prediction horizon T was 3 seconds. The value for γ was chosen so that the resulting FRTs were similar to the FRTs computed in [15] which were obtained via random sampling. Beyond our model of the agent dynamics, we also imposed hard constraints on the human control bounds to prevent unrealistic growth of the predictions. Namely, if either the magnitude of the nominal or confidence-adjusted predictions exceeded 10 m/s 2 or 2 rad/s, the prediction was capped to those limits.
In both of these scenarios, we employ a simple planner for the ego vehicle -the ego vehicle nominally moves in its lane at a constant speed. If its current nominal trajectory intersects with C over the planning horizon (3 seconds), it decelerates to stop before the intersection if possible; otherwise, it decelerates maximally until it stops. Although collision could also be avoided by steering or more sophisticated planners, we employ this simple planner to highlight the human-robot interaction aspects and because of the straight lanes in the chosen scenarios.

A. U-turn
The first case study examined dealt with a human driver executing a U-turn at an intersection. In this scenario, the ego vehicle is approaching the intersection at a speed of 21 m/s. The human vehicle begins at rest in the rightmost lane and slowly starts to execute a U-turn maneuver. Figure 1 shows the difference in behavior when using confidence-based predictions versus trusting the raw Trajec-tron++ predictions. The belief evolution for the entire time horizon of the scenario is shown in Figure 2. Here, as the human vehicle begins the U-turn from rest, Trajectron++ makes incorrect predictions that it will continue to stay in place or move straight very slowly. Whereas, actually, the human vehicle turns much faster towards the left; consequently, the model confidence falls. However, as more of the maneuver is completed, Trajectron++ makes more accurate predictions and model confidence improves.
Notably, the confidence-based predictions enable the ego vehicle to detect a potential collision 47 m away from the stop line and stop safely with a deceleration of -4.67 m/s 2 over 4.5 seconds. In contrast, when the ego vehicle does not take into account model confidence, it fails to detect potential collision until it is 5.31 m away from the stop line. The ego vehicle tries to stop by applying the maximal deceleration of -10 m/s 2 for 2.1 seconds. Despite its best efforts, the ego vehicle ends up stopping in the middle of the intersection and colliding with the human.

B. Running a Stop Sign
In this next scene, the ego vehicle is crossing an intersection at a speed of 21 m/s with the right of way. However, a human vehicle begins to enter the intersection early as the ego vehicle approaches, even though it is supposed to yield to the ego vehicle. Interestingly, this scene also has extra semantic information that leads to poor predictions -the human car is surrounded by other cars that remain stopped behind the stop line. Trajectron++ uses the history of agents in close proximity to the human to inform predictions on their control actions and believing that the human car will also stop. Thus, the robot immediately loses any confidence in the predictive model once the human begins to move forward. This is visualized in Figure 3 along with the FRTs with and without confidence. The belief is steadily regained as Trajectron++ determines the intent of the human.
Using model confidence, the ego vehicle is able to detect a potential collision after 2.5 seconds 37.3 meters away from the stop line, which is indicated by the white road line in Figure 3. This allows the ego vehicle to gracefully stop before entering the intersection, decelerating at a rate of -5.9 m/s 2 for 3.55 seconds. Without using confidence, detecting potential collision takes the robot 4 seconds and results in the robot being 5.8 m away from the stop line. This leads to the ego vehicle ending up 16.2 m past the stop line in the middle of the intersection, which leads to a collision.
Each of the case studies illustrates that the updates of the FRT with respect to both changes in predictions and changes in model confidence result in a safer behavior. The FRTs in both scenarios, despite their differing appearance, were computed by the same value function. This shows that a single parameter-conditioned FRT can be used in different scenarios for safety updates.
VII. DISCUSSION AND FUTURE WORK In this paper, we propose to use parameter-conditioned reachable sets for the purpose of safe human-robot interaction. By making a time-varying parameterization of the human controls and model confidence, we demonstrated a realtime safety assurance updates for high-capacity human-robot interaction systems. Our two running examples grounded in the autonomous driving domain illustrated how our method's ability to quickly update the forward reachable tube of the human enables a safer human-robot interaction.
In future, it would be interesting to apply our framework in the context of other human-robot interaction tasks, such as assistive manipulation and autonomous navigation. We will also investigate closed-loop human-robot interactions and operating around multiple humans.


Iterative Residual Policy: for Goal-Conditioned Dynamic Manipulation of Deformable Objects

This paper tackles the task of goal-conditioned dynamic manipulation of deformable objects. This task is highly challenging due to its complex dynamics (introduced by object deformation and high-speed action) and strict task requirements (defined by a precise goal specification). To address these challenges, we present Iterative Residual Policy (IRP), a general learning framework applicable to repeatable tasks with complex dynamics. IRP learns an implicit policy via delta dynamics -- instead of modeling the entire dynamical system and inferring actions from that model, IRP learns delta dynamics that predict the effects of delta action on the previously-observed trajectory. When combined with adaptive action sampling, the system can quickly optimize its actions online to reach a specified goal. We demonstrate the effectiveness of IRP on two tasks: whipping a rope to hit a target point and swinging a cloth to reach a target pose. Despite being trained only in simulation on a fixed robot setup, IRP is able to efficiently generalize to noisy real-world dynamics, new objects with unseen physical properties, and even different robot hardware embodiments, demonstrating its excellent generalization capability relative to alternative approaches. Video is available at https://youtu.be/7h3SZ3La-oA

I. INTRODUCTION
We study the task of goal-conditioned dynamic manipulation of deformable objects, examples of which include whipping a target point with the tip of a rope or spreading a cloth into a target pose (Fig. 1). Humans show remarkable ability to not only perform these tasks with high precision (e.g., aiming a lasso or setting a table cloth), but also to quickly transfer these manipulation skills to new objects with very few attempts. For robots, however, this has historically been a highly challenging problem due to several factors: • Complex dynamics: Unlike quasi-static manipulation, dynamic manipulations (often with high-speed actions) often lead to complex and non-linear effects. These dynamical processes are difficult to model, and pose significant challenges for system identification and state estimation. As a result, it is often infeasible to apply classical algorithms, such as optimal control, when an accurate and performant forward model of the system is prohibitively difficult to obtain. • Complex object properties: Unlike rigid objects, deformable objects exhibit dynamics that are influenced by numerous hard-to-estimate factors such as nonlinear anisotropic stiffness, friction, density distribution, and changing aerodynamics. Not only is it challenging to estimate these properties, it is hard to map them to the parameter used in common simulators using simplified physics models.

Goal-conditioned Dynamic Rope Manipulation
Goal-conditioned Dynamic Cloth Manipulation y z Fig. 1: Goal-conditioned dynamic manipulation of deformable objects. Our method achieves high accuracy by iteratively adjusting actions using visual feedback and a learned delta dynamics network. Top: Rope manipulation task. Goal configuration is defined by a target tip position (green cross). Bottom: Cloth manipulation task. Goal configuration is defined by target keypoint locations (green dots).
• Precise goal conditions: Due to these challenges, prior work that applies dynamic manipulation to deformable objects generally studies tasks with low precision requirements such as cloth unfolding [6] or cable vaulting [31]. In contrast, the tasks studied in this paper require precise actions in order to reach the specified goal conditions -for example hitting a goal location with the tip of a whip or reaching a goal configurations defined by keypoints on a cloth.
To address these challenges, we present Iterative Residual Policy (IRP), a general formulation for goal-conditioned dynamic manipulation, that learns how to iteratively improve its actions to achieve a given goal condition using visual feedback. This formulation highlights two key features: • Iterative action refinement: Instead of directly inferring the optimal action from the goal, IRP starts with a best guess action and iteratively refines it to move toward the goal. For tasks with repeatable dynamics, this iterative approach allows the system to achieve high precision and be robust against errors from action, observation, and model prediction. predict an updated trajectory from an observed trajectory with small action perturbations. Compared to modeling the entire dynamical system (action in, trajectory out), this "delta dynamics" (especially its general direction) is much easier to predict and generalize to new objects, thereby, enabling quick online adaption. Our primary contribution is the Iterative Residual Policy (IRP), a general learning framework for goal-conditioned dynamic manipulation. Despite only trained using simulation data, IRP can be directly applied to real-world hardware. Furthermore, IRP demonstrates impressive generalization capability to unseen object instances, out-of-distribution rope parameters, unmodeled physical effects, and varying robot embodiments. Our experiments show that IRP can achieve pixel level accuracy for a wide variety of goals in both simulation and real robot environments (1.8cm and 2.6cm respectively).

II. RELATED WORK
In this section, we will focus on summarizing relevant prior methods for deformable object manipulation.
Goal-conditioned manipulation is particularly difficult for deformable objects due to their high degree-of-freedom and under-actuation [19,20,23,24,8,22]. Previous work studied tasks such as rope knot-tying [24] and fabric folding [8]. Early methods attempt to transfer demonstration trajectories to unseen configurations [22]. Recently, deep learning has been used extensively [23,24,8] to enable policy generalization to unseen objects or multiple tasks. However, these methods only consider quasi-static actions, where action effects are inferred mostly from object geometry alone. While quasi-static manipulations has shown to be sufficient to solve many tasks, the resulting systems are often slow and inefficient.
Dynamic manipulation takes advantage of momentum (in addition to kinematic, static and quasi-static mechanisms) to increase load capacity or enable motions to have manipulands extend outside of the robot's nominal reach range [16,30,4,5,21,28,11]. Deformable objects pose a unique generalization challenge for dynamic manipulation policies due to complex physical properties and a particularly large sim2real gap. Using reinforcement learning, Jangir et al. [9] proposes a system to dynamically fold and place cloth in various goal configurations. However, thousands of interactions are required for training. Similarly, Lim et al. [13] closes the sim2real gap by algorithmically tuning physics simulation parameters with realworld observations. But this process needs to be repeated for every unseen object instance. Leveraging visual feedback and domain randomization, Hietala et al [7] is able to successfully transfer a manipulation policy trained in simulation to a real robotic setup. However, the policy is limited to a narrow range of goals such as folding clothes exactly in half. In comparison, our method is able to generalize to both unseen real-world object instances as well as wide variety of goals, making it a step closer to real-world applications.
Trajectory optimization uses analytical models [28] or numerical simulation [12,18,29,10,25] to generate mostly open-loop solutions for deformable object manipulation. Once a model has been designed, or automatically identified [25], the optimization problem can be solved using methods such as direct collocation [10], single shooting [12] or black-box optimization [18]. These methods are generally capable of handling a wide range of goals and generalizing to many object instances, as long as a model is available for each new object. However, direct execution of optimal trajectory solutions on hardware (i.e., the OptSim method described later) does not work well for our tasks due to the large sim2real gap. In contrast, our method bridges this gap using feedback from previous trajectories without an explicit dynamics model.
Iterative Learning Control (ILC) leverages feedback from previous iterations to improve the accuracy of a repeatable system [17,1]. Most ILC algorithms tackle the task of trajectory tracking control, compensating for repeated disturbances against a reference trajectory. However, when the objective function is non-convex and difficult to optimize (like the two tasks we investigate in this paper), the reference trajectory is hard to obtain. Hence, ILC can not be directly applied. In addition, existing ILC algorithms often assumes known control direction or Jacobian which is difficult to obtain for our task. In contrast, our method is able to iteratively solve these tasks despite its complexity.

III. PROBLEM SETUP
We use two domains as examples of general dynamic manipulation tasks, one featuring 1-D deformable objects (ropes) and the other focusing on 2-D deformable objects (table  cloths). For both tasks, the physical parameters of the objects are not known to the algorithm a priori. During testing, a single policy is used for each task and evaluated across a diverse set of manipulands and robot embodiments, demonstrating our method's strong generalization capability to objects significantly outside of its training distribution. Rope whipping. The task is to hit a target location in the air with the tip of a rope attached to a UR5 robot (Fig 1 first  row). The range of target locations far exceed the robot's reach range, requiring the robot to swing the rope dynamically. To reach sufficient speed in practice, we extended the UR5's endeffector with a 50cm long wooden stick. We use a parametric action primitive to describe the robot's movement. The action space for the whipping primitive is a = (v, • are the target angles for joints two and three, respectively, and v ∈ [1.0, 3.14] rad/s is the maximum permissible angular velocity across all joints. This action primitive considers only 2D movement in the Y-Z plane, where target locations are defined in-plane -it is sufficient for this task, since out-of-plane goals could be reached simply by rotating the robot's base joint. The trajectory of the rope tip T i is tracked and rasterized to a 256 2 image as observation input. The distance metric D(T i , g) for this task is defined as the minimum distance from any point on the trajectory T i to the goal location g. Cloth placement. The task is to place a square cloth on the table that reaches the goal configuration specified by 9 keypoints on the cloth (shown in Fig. 1 bottom row). Again, the goal configurations are further than the arm's maximum reach range, requiring dynamic actions. We assume that the cloth is gripped by two grippers that move in sync and consider the case where the grippers only move within the Y-Z plane, since out-of-plane goals can be reached by horizontally translating the trajectory. Gripper trajectory is parameterized as a cubic spline with 3 via-points, evenly spaced temporally, in the Y-Z plane, Fig. 4. The start point and the Z coordinate for the end point are fixed. The remaining 3 degrees of freedom -Y,Z locations for the second via-point and the Y locations of the third via-point-as well as the total duration of the action, constitute the 4 dimensional action space. The trajectory for all 9 keypoints on the cloth T i is used as input observation to the algorithm. The distance metric D(T i , g) for this task is defined as the mean keypoint distance between the cloth's final configuration and target configuration.

IV. APPROACH
When attempting to hit a target with an unfamiliar rope, humans will generally not succeed on their first try. However, using physical intuition, people can usually predict the effect of adjusting their actions; when swinging a rope, for example, larger force will make the rope tip swing higher. Although the prediction is not perfect, people often adjust their actions in §III Defined for each task 6: if d i < d stop then j * = arg min j (D(T j i ,g)) 12: a i+1 = a i + δa j * i 13: end while the right direction and quickly drive down their error after a few interactions.
Building on this intuition, we propose Iterative Residual Policy for goal-conditioned dynamic manipulation of deformable objects. Given an observation of trajectory T i by action a i , we sample a set of potential delta actions δa i and predict the resulting trajectoryT i for each delta action. We then evaluate each of the predicted trajectories based on their minimal distance to the goal d i , and greedily select the optimal delta action δa j * i to execute in the next iteration. Trained using simulation data alone, this method is able to achieve high accuracy and generalize to out-of-distribution objects on our real-world robot setup. The following sections provide details for the key algorithm components and design decisions.  Fig. 4: Cloth placing task A) Some goals are placed outside the arm's reach range, therefore requiring dynamic actions. B) Top row: our algorithm adjusts the trajectory to reach a far-away goal. Bottom row: our algorithm reduces action velocity to eliminate undesired folding. C) Mean distance to goal keypoints averages across all test cloth and goals, shown with a 95% confidence interval. Our method yields significantly better performance compared to the [deltaReg] baseline. Note that due to the stretching of the cloth and thickness of collision capsules, it is not possible to achieve 0 error.

A. Delta Dynamics Network
The delta dynamics network takes in an observed trajectory T i and a delta action δa j i as input and predicts the trajectorŷ T j i after the delta action is applied. This network is used at every iteration for action optimization.
Trajectory representation. To spatially represent the trajectory T i , we rasterize the it into a 256 × 256 image projected onto the Y-Z plane. T i covers ±3m area from the robot base joint. The pixel values correspond to occupancy probability; the observed trajectories have a binary pixel and the predicted trajectories are real-valued: p ∈ [0, 1]. For the cloth placing task, the trajectory image of all 9 keypoints are stacked channelwise. Due to small differences in initial condition, hardware precision, ambient airflow as well as other disturbances, the trajectory is not always the same for each action. Moreover, different segments of the trajectory exhibit different sensitivity to noise.
Action representation. We broadcast the N a dimensional delta action δa i into a N a channel image and then concatenate them with the trajectory images before feeding them into the network. This spatially aligned representation allows us to use a standard CNN architecture designed for image segmentation while ensuring the action information is available in the receptive field of every neuron.
Network and loss. We use DeepLabV3+ [2] for our network architecture. During training, we uniformly select actions as a i and sample δa i with a Gaussian distribution (SD=0.125). The resulting trajectories in simulation are used as supervision. The network is trained with Binary Cross Entropy Loss and the AdamW optimizer [15] with learning rate of 0.001 and weight decay of 1 × 10 −6 .

B. Interactive Action Sampling and Selection
Initial action. In the rope whipping task, the initial action a 0 is selected using the best average action for all training rope given a specific goal. This action can be computed efficiently with our offline training dataset. The same action is used for a goal regardless of the rope parameters. In the cloth placement task, a constant initial action is used for all goals to ensure all keypoint trajectories are observable in the first iteration.
Delta action. Since each dimension of the action space has been scaled to between 0 and 1, we sample N s = 128 delta actions δa i for each iteration from a spherical Gaussian distribution with 0 mean and standard deviation σ. To reduce overshooting and accelerate convergence, Action selection. After the network predicts the raw trajectories image for all delta action sample δa 0:N i , we threshold the prediction with t = 0.2, creating a set of binary trajectory imageT 0:N i . Then, the delta action δa j * i associated with smallest distance D(T j i , g) is selected to compute next action: a i+1 = a i + δa j i . In real-world experiments, the optimization stops when the policy reaches d stop = 2cm error or maximum iteration max step = 10 is reached. In simulation, the optimization executes for exactly 16 iterations. The algorithm is summarized as pseudocode in Algorithm 1.

C. Training Data Generation
Rope whipping. For training and validation, we built a simulation environment in MuJoCo [26]. The rope is simulated using 25 linked capsules and to generate different ropes, varying two parameters: length and density. The range of both parameters as well as the training/testing split is shown in Fig. 6 left. The rope parameters are sampled from a 12 2 grid. For each rope, we simulate 50 3 actions within the set box constraint for speed [1, 3.14] rad/s, joint 2 ∈ [90, −30] • and joint 3 ∈ [−110, −290] • . In addition, each action was repeated 3 times with different random perturbations on the initial state to capture the stochastic nature of the trajectory. The entire dataset contains 54 million trajectories.
Clearly this model does not capture all the variations in real-world ropes, and the simulation itself presents different dynamics from the real world due to unmodeled factors such as aerodynamics, collision with the floor, etc. Despite the significant sim-to-real gap, we will later show that our model trained on these simulated ropes generalizes very well in the real world with significantly out-of-distribution ropes. Cloth placement. Similarly, the training data for cloth is collected from a MuJoCo simulation environment. The square cloth is simulated as a 13 2 grid of capsules, skinned as a cloth for visualization. We again vary 2 parameters: size (ranged between 0.4 to 0.6 m) and density (ranged between 0.2 to 1.4 kg/m 2 ). For each cloth parameter, we sample speed and 3 via-point parameters from a 8 × 16 3 grid. In total, the dataset contains 131 thousand examples.

D. Real world system setup.
We conducted real-world experiments for the rope whipping task with an UR5-CB3 robot augmented with a wooden extension. We used a single RGB camera (Stereolab ZED 2i) running at 720p 60fps for tracking the tip of the rope. Due to the limited resolution and large motion range, stereo tracking does not provide sufficient depth accuracy and reliability. We therefore assume that the rope tip only moves in a 2D plane, allowing us to reduce the perception problem to that of 2D tracking. We placed the camera 2.4m away from the robot, and calibrated the homography between the image plane and the robot Y-Z plane with manually labeled point correspondences. We trained a keypoint detection model based on DeepLabV3+ with 400 hand labeled images. Note that we use a higher image resolution (720p instead of 256p) for tracking results with less than 1.3cm tracking error in the validation set.

V. EVALUATION
In our experiments we seek to validate the system's generalization capability to 1) novel object physical parameters 2) real-world dynamics, and 3) robot hardware embodiment. In all following experiments, both in simulation and the real-world, we use the same model trained with simulation training ropes generated in Sec. IV-C. The cloth placing task is evaluated in simulation only and is discussed in Sec. VI

A. Rope Whipping Task
Metrics. We measure the performance of these algorithms using the minimum distance to the goal (in cm) at each step. The faster that an algorithm reaches a certain distance the better. We allow the maximum iteration to be 16 in simulation and 10 in real-world experiments. Note that the pixel size is around 2.3cm in our trajectory image encoding and the real-world tracking accuracy is around 1.3cm.
Test Cases. To test the system's ability to generalize to different rope parameters, we used the following testing ropes: • Simulated ropes with parameters in the interpolation regime relative to the training rope parameters (Fig. 6 green). • Simulated ropes with parameters in the extrapolation regime relative to the training rope parameters (Fig. 6 blue). • Real-world ropes with different material, length, and mass distribution (Fig. 5 C). We modeled the simulation rope after the [base-rope], therefore all other ropes are significantly out-of-distribution. In particular, the [bullwhip], and [knottedrope] have a non-uniform mass distribution and the [longcloth] has a larger surface area to density ratio, hence, experiences much larger aerodynamic effects (unmodeled in simulation) than to other ropes.
Robot hardware embodiment. We validate the system's generalization capability to different robot hardware embodiments by sampling the robot's last link length from 3 different lengths, [elink-x] where x = 40cm,50cm,60cm. By changing robot's last link, we are effectively changing the mapping between the robot's action parameter and its physical embodiment, which require the system to adapt according to the new trajectory observations. Fig. 7 shows the example results for this experiment, where IRP converges to 3 different actions for the same goal.
Algorithm comparisons. To validate the major design decisions and their impacts on performance, we compare with following alternative approaches: • System identification (SysID, SysID GT): This method first identifies key system parameters and then infers the action using these parameters. We give this baseline a head start by providing the true varying parameters in simulation: length and density. To ensure fairness, we use 16 fixed system identification actions to collect observations. Another 4-layer MLP is trained to regress the optimal action from estimated rope parameters. [SysID GT] is a variant of this baseline, where we assume the ground truth rope parameters are given. Iteration 0 Iteration 1 Iteration 3 Fig. 7: Adaptation to robot embodiment. We varies the robot's last link with 3 different lengths (40,50,60cm), which effectively changes the mapping between the actions and their effects. As expected, the robot with a different link than training (elink-40/-60) has a higher initial error. However, for all variations, the system is able to reach the goal by adjusting its action according to the visual feedback.
• Iterative control with heuristic (iterHeuristic): A heuristic strategy that increases both the speed and amplitude of the action if the trajectory intersects with the line segment between the goal and the origin (the goal is outside the trajectory) and decreases otherwise. If the trajectory does not intersect with the ray from the origin to the goal, the algorithm terminates to prevent increasing error. • Iterative control with linear model [iterLinear]: Instead of leveraging the residual dynamics model learned from offline data, a linear model of the plant is fitted on the trajectories observed so far, updated at each step. The action is adjusted at each step by minimizing the shortest distance to the goal using the linear model. • Direct delta action regression [DeltaReg]. Instead of using sampled delta actions as input, this method directly regresses the optimal delta action for the goal and the observed trajectory.  are particularly out-of-distribution. Our method significantly outperforms the other approaches we evaluated, demonstrating surprisingly strong generalization to unseen rope parameters, action definitions and simulation/reality divergence. IL: iterLinear. X-3, X-9 error measured in iteration 3 and 9. it with the same number of iterations. • No adaptive action sampling [ConstSigma]. An ablation of our method which uses a fixed sigma for action sampling. • Average action in simulation (AVG): The action that minimizes the average distance to goal in the training set, regardless of rope parameters, this is also the action we used to initialize our method at step one. • Optimal control in simulation (OptSim): This method estimates optimal actions in simulation with measured rope parameters from a real-world (i.e., length, density and width). This method represents an oracle for model-based control using our simulator. It is used only in real-world experiments to quantify the sim2real gap.

B. Experimental Analysis
Benefits of learning residual dynamics. We hypothesize that this formulation eases the learning process and maintains the flexibility of representing complex non-linear dynamics. To validate this hypothesis, we compared to other alternatives for modeling the system. In this example, the rope trajectory switches between several discrete modes. As a result, IterHeuristic gets stuck in a sub-optimal orbit and iterLinear failed to approximate the switching behavior. In contrast, our method accurately predicts the switching behavior and reaches the goal. Compared to the system identification method [SysID, SySId GT], IRP can better handle unmodeled physical parameters presented in real-world ropes (e.g., aerodynamics or stiffness), which is captured in the input trajectory but not in the predefined system parameters (e.g., length and density). As a result, the algorithm can adapt to real-world ropes much better than SysID methods.
Compared to [DeltaReg], IRP better models the multi-modal aspect of the solution space. Oftentimes there are multiple actions could reach the same goal, where [DeltaReg] tends to predict the mean of all valid solution (due to its MSE objective), which is likely to be an invalid solution.
Compared to [IterHeuristic], our learned IRP model can capture complexity in the rope dynamics, such as mode switching when crossing key velocity thresholds, that the heuristic approach cannot capture. The [IterHeuristic] policy essentially assumes that the tip of the rope swings in a full circle, with its radius controlled by the action's energy. However, when the energy is insufficient for the tip to swing past apex this assumption is incorrect, and therefore, lead to incorrect action prediction. Moreover, around the apex point, the rope trajectory quickly switches between several modes, causing [IterHeuristic] to get stuck in oscillating local minima (Fig. 8  first row).
Finally, compared to [iterLinear]: our IRP model is able to converge in fewer steps with lower distance. In many cases, the rope trajectories switch between several discrete modes for different actions in a highly non-linear fashion, which cannot be easily captured by the linear model ( Fig. 8 second row). In contrast, our model is able to learn a non-linear model from Knotted Rope Base Rope Distance (cm) Iteration Fig. 9: Online adaptation to system changes. In this experiment, the robot first interacts with [base-rope-100]. At step 6, the rope is knotted. We plot the distance to goal with respect to the steps. While the tied knots significantly change the rope's length, density and mass distribution, the system is able to quickly adjust to the new system dynamics and achieve low errors. the diverse set of training trajectories.
Benefits of iterative action refinements. With its iterative refinements, IRP can consistently improve its performance from the best average action. On average, the error drops 94% on interpolation experiments and 91% drop on extrapolation experiments by using the additional trajectory observations. Moreover, the comparison with [ConstSigma] shows that the adaptive action sampling method (described in Sec. IV-B) can further improve the action samples and prevent overshooting around the optimal action -demonstrated by higher error and variance for [ConstSigma] in Fig. 6 C after step 4.
Comparison to optimal control in simulation. By computing the action using exhaustive search with manually measured parameters, the performance of [OptSim] represents an oracle for trajectory optimization using our MuJoCo simulation environment, and it should achieve perfect result in the simulated environments. However, due to the instability of the dynamical system and unmodeled effects such as aerodynamics, [OptSim] still results in higher error compared to IRP in our real-world experiment (Fig 10, Tab. I). This result demonstrates the importance of online adaptation for closing sim2real gaps, especially for complex dynamical systems such as the dynamic manipulation of deformable objects. Online adaptation to system changes. In this experiment, we stress test IRP' robustness against unexpected system changes during the interaction steps. We first ask the robot to interact with the [base-rope-100]. After step 6, we tie four knots on the rope and observe the system behavior. While the tied knots significantly changes the rope's length, density and mass distribution, we observe that the system is able to quickly adjust to the new system dynamics and regain good performance ( Fig. 9). Real-world test results on different ropes. Using the same network trained on simulation data only, our method is able to generalize to significantly out-of-distribution ropes and a wide range of goals. First row: step 0,1,3,5 and 9 for long-cloth test case. Second row: step 0, 9 for other test cases. OptSim is not able to achieve low error, indicating the large sim2real gap in this task. All distances are shown in log scale. iterLinear failed to find good solutions within 9 steps, since it uses online observations only and has limited model capacity. More results can be find in the supplemental video.

VI. EXTENSION TO CLOTH MANIPULATION
To demonstrate the generality of IRP's, we applied the same method to a cloth placement task with minimal modifications.
Test cases. We randomly sampled five unseen cloth parameter pairs (size, density) as our testing cases. For each cloth sample, we uniformly sampled 11 goal configurations from the range illustrated in Fig. 4 A. Note that since test cloths all have different sizes, the specific target keypoint locations are adjusted accordingly.
Metrics. The performance is measured by the average distance to the goal (in cm) over all keypoints. Note that due to the stretching effects on the cloth and the thickness of MuJoCo's capsule model for collision, it is not possible to reach zero error for a target configuration with perfectly square and flat keypoint locations.
Result . Fig 4 B and C show the qualitative and quantitative results respectively . Fig 4 B shows that IRP can adjust the action to different landing locations and shape of the cloth (e.g. to avoid folding). We compared with [DeltaReg] and [IterHeuristic], two of the best performing baseline methods from the rope whipping task. We modified the [IterHeuristic] to increase all action parameters if the landing keypoints are closer than the goal, and decreases otherwise, with the proportional gain set to 0.5. On average, IRP achieves 3.5 cm error across 11 test goal configurations, yielding better performance and lower variance comparing to other methods.

VII. CONCLUSION
This paper presents a general learning framework for goalconditioned dynamic manipulation: Iterative Residual Policy. Our extensive experiments in both simulation and the realworld demonstrate its adaptability to many aspects of the system, including object parameters, real-world dynamics, and even robot hardware embodiment. However, our method is not without limitations, one of which is the assumption of action repeatability, which can not be guaranteed in many applications. We also assume full observability of the manipuland throughout the trajectory, which makes direct application of this approach difficult in highly cluttered scenarios. Future work could explore using different visual feedback [14,3] to deal with non-repeatable action scenarios and perception techniques leveraging temporal cues to handle partial observability [27].