LS 3 : Latent Space Safe Sets for Long-Horizon Visuomotor Control of Iterative Tasks
Anonymous Author(s)
AfﬁliationAddress email
Abstract:
Reinforcement learning (RL) has shown impressive success in exploring high-dimensional environments to learn complex tasks, but can often exhibitunsafe behaviors and require extensive environment interaction when explorationis unconstrained.
A promising strategy for learning in dynamically uncertain environments is requiring that the agent can robustly return to learned Safe Sets,where task success (and therefore safety) can be guaranteed.
While this approachhas been successful in low-dimensions, enforcing this constraint in environmentswith visual observation spaces is exceedingly challenging.
We present a novelcontinuous representation for Safe Sets framed as a binary classiﬁcation problemin a learned latent space, which ﬂexibly scales to high-dimensional image observations. We then present a new algorithm, Latent Space Safe Sets (LS 3 ), whichuses this representation for long-horizon control.
We evaluate LS 3 on 4 domains,including a challenging sequential pushing task in simulation and a physical cablerouting task.
We ﬁnd that LS 3 can use prior task successes to restrict explorationand learn more efﬁciently than prior algorithms while satisfying constraints.
Seehttps://tinyurl.com/latent-safe-sets for supplementary material.
Introduction
Visual planning over learned forward dynamics models is a popular area of research in roboticcontrol from images [1, 2, 3, 4, 5, 6, 7], as it enables closed-loop, model-based control for taskswhere the state of the system is not directly observable or difﬁcult to analytically model, such as theconﬁguration of a sheet of fabric or segment of cable.
These methods learn predictive models overeither images or a learned latent space, which can then be used to plan actions which maximize sometask reward.
While these approaches have signiﬁcant promise, there are several open challenges inlearning policies from visual observations.
First, reward speciﬁcation is particularly challenging forvisuomotor control tasks, because high-dimensional observations often do not expose the necessaryfeatures required to design informative reward functions [8], especially for long-horizon tasks.
Second,while many prior reinforcement learning methods have been successfully applied to image-basedcontrol tasks [9, 10, 11, 12, 13], learning policies from image observations often requires extensiveexploration due to the high dimensionality of the observation space and the difﬁculties in rewardspeciﬁcation, making safe and efﬁcient learning exceedingly challenging.
One promising strategy for efﬁciently learning safe control policies is to learn a Safe Set [14], whichcaptures the set of states from which the agent has previously completed the task. When used torestrict exploration, this Safe Set can be used to enable highly efﬁcient and safe learning [15, 16, 14],as exploration is restricted to states in which the agent is conﬁdent in task success.
However, whilethese Safe Sets can give rise to algorithms with a number of appealing theoretical properties such asconvergence to a goal set, constraint satisfaction, and iterative improvement [14, 17, 15], using themfor controller design for practical problems requires developing continuous approximations at theexpense of maintaining theoretical guarantees [16].
This choice of continuous approximation is a keyelement in determining the applications to which these Safe Sets can be used for control. Prior works have presented approaches which collect a discrete safe set of states from previouslysuccessful trajectories and represent a continuous relaxation of this set by constructing a convex hull
[Figure]
of these states [14] or via kernel density estimation with a tophat kernel function [16].
While theseapproaches have been successful for control tasks with low-dimensional states, extending them tohigh-dimensional observations presents two key challenges: (1) scalability: these prior methodscannot be efﬁciently applied when the number of observations in prior successful trajectories is large,as querying safe set inclusion scales linearly with number of samples it contains and (2) representationcapacity: both of these prior approaches do not scale well to high dimensional observations and arelimited in the space of continuous sets that they can efﬁciently represent.
Applying these ideas tovisuomotor control is even more challenging, since images do not directly expose details about thesystem state or dynamics that are typically needed for formal controller analysis [15, 14, 18].
This work makes several contributions. First, we introduce a novel, scalable continuous approximationmethod which makes it possible to leverage Safe Sets for visuomotor policy learning. The key ideais to reframe the safe set approximation as a binary classiﬁcation problem in a learned latent space,where the objective is to distinguish states from prior successful trajectories from those in priorunsuccessful trajectories.
Second, we present Latent Space Safe Sets (LS 3 ), a model-based RLalgorithm which encourages the agent to maintain plans back to regions in which it is conﬁdent intask completion, even when learning in high dimensional spaces.
This constraint makes it possibleto deﬁne a control strategy to (1) improve safely by encouraging consistent task completion (andtherefore avoid unsafe behavior) and (2) learn efﬁciently since the agent only explores promisingstates in the immediate neighborhood of those in which it was previously successful.
Third, wepresent simulation experiments on 3 continuous control visuomotor control tasks which suggest that
LS 3 can learn to improve upon demonstrations more safely and efﬁciently than prior algorithms. Fourth, we conduct physical experiments on a vision-based cable routing task on the da Vinci surgicalrobot indicate that LS 3 can learn a policy more efﬁciently than prior algorithms while consistentlycompleting the task and satisfying constraints during learning.
Related Work2.1 Safe, Iterative Learning Control
In iterative learning control (ILC), the agent tracks an initially provided reference trajectory and usesdata from controller rollouts to iteratively reﬁne tracking performance [19].
Rosolia et al.
[20], Rosoliaand Borrelli [17, 14] present a new class of algorithms, known as Learning Model Predictive Control(LMPC), which are reference-free and instead iteratively improve upon the performance of aninitial feasible trajectory.
To achieve this, Rosolia et al.
[20], Rosolia and Borrelli [17, 14] presentmodel predictive control algorithms that use data from controller rollouts to learn a Safe Set andvalue function, with which recursive feasibility, stability, and local optimality can be guaranteedgiven a known, deterministic nonlinear system or stochastic linear system under certain regularityassumptions.
However, a core challenge with these algorithms is that they assume known systemdynamics, and cannot be applied to high-dimensional control problems.
Thananjeyan et al.
[16]extends the LMPC framework to higher dimensional control settings in which system dynamics areunknown and must be estimated from data, but the visuomotor control setting introduces a number of new challenges for iterative learning control algorithms such as learning system dynamics, Safe Sets,and value functions which can ﬂexibly and efﬁciently accommodate visual inputs.
2.2 Model Based Reinforcement Learning
There has been signiﬁcant recent progress in algorithms which combine ideas from model-basedplanning and control with deep learning [21, 22, 23, 24, 25, 26].
These algorithms are gainingpopularity in the robotics community as they enable leaning complex policies from data whilemaintaining some of the sample efﬁciency and safety beneﬁts of classical model-based controltechniques.
However, these algorithms typically require hand-engineered dense cost functions fortask speciﬁcation, which can often be difﬁcult to provide, especially in high-dimensional spaces.
This motivates leveraging demonstrations (possibly suboptimal) to provide an initial signal regardingdesirable agent behavior.
There has been some prior work on leveraging demonstrations in modelbased algorithms such as Quinlan and Khatib [27] and Ichnowski et al.
[28], which use modelbased control with known dynamics to reﬁne initially suboptimal motion plans, and Fu et al.
[23],which uses demonstrations to seed a learned dynamics model for fast online adaptation usingiLQR [23].
Thananjeyan et al.
[16], Zhu et al.
[29] present ILC algorithms which rapidly improveupon suboptimal demonstrations when system dynamics are unknown.
However, these algorithmseither require knowledge of system dynamics [27, 28] or are limited to low-dimensional statespaces [23, 16, 29] and cannot be ﬂexibly applied to visuomotor control tasks.
2.3 Reinforcement Learning from Pixels
Reinforcement learning and model-based planning from visual observations is gaining signiﬁcantrecent interest as RGB images provide an easily available observation space for robot learning [1, 30].
Recent work has proposed a number of model-free and model-based algorithms that have seen successin laboratory settings in a number of robotic tasks when learning from visual observations [31, 32,10, 33, 12, 13, 1, 34, 30].
However, two core issues that prevent application of many RL algorithmsin practice, inefﬁcient exploration and safety, are signiﬁcantly exacerbated when learning fromhigh-dimensional visual observations in which the space of possible behaviors is very large and thefeatures required to determine whether the robot is safe are not readily exposed.
There has beensigniﬁcant prior work on addressing inefﬁciencies in exploration for visuomotor control such aslatent space planning [2, 30, 34] and goal-conditioned reinforcement learning [13, 10].
However, safereinforcement learning for visuomotor tasks has received substantially less attention.
Thananjeyanet al.
[35] and Kahn et al.
[36] present reinforcement learning algorithms which estimate the likelihoodof constraint violations to avoid them [35] or reduce the robot’s velocity [36].
Unlike these algorithms,which focus on presenting methods to avoid violating user-speciﬁed constraints, LS 3 additionallyprovides consistent task completion during learning by limiting exploration to the neighborhood ofprior task successes.
This difference makes LS 3 less susceptible to the challenges of unconstrainedexploration present in standard model-free reinforcement learning algorithms.
Problem Statement
We consider an agent interacting in a ﬁnite horizon goal-conditioned Markov Decision Processes(MDP) which can be described with the tuple M =
( S , G , A , P ( ·|· , · ) , R ( · , · ) , µ, T ) .
S and A are 119 the state and action spaces, P : S ⇥ A ⇥ S ! [0 , 1] maps a state and action to a probabilitydistribution over subsequent states, R : S ⇥
A ⇥ S ! R is the reward function, µ is the initialstate distribution ( s 0 ⇠ µ ), and T is the time horizon.
In this work, the agent is only provided with
RGB image observations s t 2 R W ⇥ H ⇥ 3 + = S , where W and H are the image width and height inpixels, respectively.
We consider iterative tasks, where the agent must reach a ﬁxed goal set G ✓ S as 124 efﬁciently as possible and the support of µ is small.
While there are a number of possible choices ofreward functions that would encourage fast convergence to G , providing shaped reward functionscan be exceedingly challenging, especially when learning complex tasks in which agents are onlyprovided with high dimensional observations.
Thus, as in Thananjeyan et al.
[16], we consider asparse reward function that only provides a signal upon task completion: R ( s, a, s 0 )
=[-
-]
if s 0 2 G and 1 otherwise.
To incorporate constraints, we augment M with an extra constraint indicator function C : S ! { 0 , 1 } which indicates whether a state satisﬁes user-speciﬁed state-space constraints, such 131 as avoiding known obstacles.
This is consistent with the modiﬁed CMDP formulation used in [35].
We assume that R and C can be evaluated on the current state of the system, but may be approximatedusing prior data for use during planning.
We make this assumption because in practice we plan overpredicted future states, which may not be predicted at sufﬁciently high ﬁdelity to expose the necessaryinformation to directly evaluate R and C during planning.
[Figure]
Given a policy ⇡ : S ! A , its expected total return in M can be deﬁned as R ⇡ = E ⇡ ,µ,P [ P t R ( s t , a t )] .
Furthermore, we deﬁne P ⇡ C ( s ) as the probability of future constraint violation (within time horizon T ) under policy ⇡ from state s .
Then the objective is to maximize theexpected return R ⇡ while maintaining a constraint violation probability lower than some  C .
Thiscan be written formally as follows:
[Equation]
We assume that the agent is provided with an ofﬂine dataset D of transitions in the environment ofwhich some subset D constraint ( D are constraint violating and some subset D success ( D appear in 143 successful demonstrations from a suboptimal supervisor.
As in [35], D constraint contains examples of 144 constraint violating behaviors (for example from prior runs of different policies or collected underhuman supervision) so that the agent can learn about states which violate user-speciﬁed constraints.
Latent Space Safe Sets (LS 3 )
Here we describe how LS 3 uses demonstrations and online environment interaction to safely learniteratively improving policies.
Section 4.1 describes how we learn a low-dimensional latent representation of image observations to facilitate efﬁcient model-based planning.
To enable this planning, welearn a probabilistic forward dynamics model as in [25] in the learned latent space and models toestimate whether plans will likely complete the task (Section 4.2) and to estimate future rewards andconstraint violations (Section 4.3) from predicted trajectories.
Finally, in Section 4.4, we discuss howall of these components are combined in LS 3 to enable safe and efﬁcient policy improvement.
Dataset
D is expanded using online rollouts of LS 3 and used to update all latent space models (Sections 4.2and 4.3) after every K rollouts.
See Algorithm 1 and the supplement for further details on trainingprocedures and data collection for all components.
4.1 Learning a Latent Space for Planning
Learning compressed representations of images has been a popular approach in vision based controlto facilitate efﬁcient algorithms for planning and control which can reason about lower dimensional inputs [2, 34, 6, 37, 38, 30].
To learn such a representation, we train a  -variational autoencoder [39]on states in D to map states to a probability distribution over a d -dimensional latent space Z .
Theresulting encoder network f enc ( z | s ) is then used to sample latent vectors z t ⇠ f enc ( z t | s t ) to traina forward dynamics model, value function, reward estimator, constraint classiﬁer, Safe Set, andcombine these elements to deﬁne a policy for model-based planning.
Motivated by Laskin et al.[40], during training we augment inputs to the encoder with random cropping, which we found to behelpful in learning representations that are useful for planning.
For all environments we use a latentdimension of d = 32 , as in [2] and found that varying d did not signiﬁcantly affect performance.
4.2 Latent Safe Sets for Model-Based Control
LS 3 learns a binary classiﬁer for latent states to learn a latent space Safe Set that represents statesfrom which the agent has high conﬁdence in task completion based on prior experience.
Because theagent can reach the goal from these states, they are safe: the agent can avoid constraint violations bysimply completing the task as before.
While classical algorithms use known dynamics to construct
Safe Sets, we approximate this set using successful trajectories from prior iterations.
At each iterationj , the algorithm collects K trajectories in the environment.
We then deﬁne the sampled Safe Set atiteration j , S j , as the set of states from which the agent has successfully navigated to G in iterations0 through j of training, where demonstrations trajectories are those collected at iteration 0 .
Werefer to the dataset collecting all these states as D success .
This discrete set is difﬁcult to plan to withcontinuous-valued state distributions so we leverage data from D success (data in the sampled Safe Set),data from D \ D success (data outside the sampled Safe Set), and the learned encoder from Section 4.1 180 to learn a continuous relaxation of this set in latent space (the Latent Safe Set).
We train a neuralnetwork with a binary cross-entropy loss to learn a binary classiﬁer f S ( · ) that predicts the probabilityof a state s t with encoding z t being in S j .
To mitigate the negative bias that appears when trajectoriesthat start in safe regions fail, we utilize the intuition that if a state s t +1 2 S j then it is likely that s t isalso safe.
To do this, rather than just predict S j , we train f S with a recursive objective to predictmax( S j ,  S f S ( s t +1 )) .
The relaxed Latent Safe Set is parameterized by the superlevel sets of f S ,where the level  S is adaptively set during execution:
S j Z = { z t | f S ( · )( z t )   S } .
4.3 Reward and Constraint Estimation
In this work, we deﬁne rewards based on whether the agent has reached a state s 2 G , but we needrewards that are deﬁned on predictions from the dynamics, which may not correspond to valid realimages.
To address this, we train a classiﬁer f G : Z ! { 0 , 1 } to map the encoding of a state to 191 whether the state is contained in G using terminal states in D success (which are known to be in G ) and 192 other states in D .
However, in the temporally-extended, sparse reward tasks we consider, rewardprediction alone is insufﬁcient because rewards only indicate whether the agent is in the goal set, andthus provide no signal on task progress unless the agent can plan to the goal set.
To address this, asin prior MPC-literature [16, 15, 14, 8], we train a recursively-deﬁned value function (details in thesupplement).
Similar to the reward function, we use the encoder (Section 4.1) to train a classiﬁerf C : Z ! [0 , 1] with data of constraint violating states from D constraint and the constraint satisfying 198 states in D \ D constraint to map the encoding of a state to the probability of constraint violation.
4.4 Model-Based Planning with LS 3
LS 3 aims to maximize total rewards attained in the environment while limiting constraint violationprobability within some threshold  C (equation 1).
We optimize an approximation of this objectiveover an H -step receding horizon with model-predictive control.
Precisely, LS 3 solves the followingoptimization problem to generate an action to execute at timestep t :
[Equation]
In this problem, the expectations and probabilities are taken with respect to the learned, probabilisticdynamics model f dyn ( z t +1 | z t , a t ) .
The optimization problem is solved approximately using the crossentropy method (CEM) [41] which is a popular optimizer in model-based RL [42, 16, 15, 43, 35].
The objective function is the expected sum of future rewards if the agent executes a t : t + H  1 andthen subsequently executes ⇡ (equation 2).
First, the current state s t is encoded to z t (equation 3).
Then, for a candidate sequence of actions a t : t + H  1 , an H -step latent trajectory { z t +1 , . . .
, z t + H } is 210 sampled from the learned dynamics f dyn (equation 4).
LS 3 constrains exploration using two chanceconstraints: (1) the terminal latent state in the plan must fall in the Safe Set (equation 5) and (2)all latent states in the trajectory must satisfy user-speciﬁed state-space constraints (equation 6).
Z Cis the set of all latent states such that the corresponding observation is constraint violating.
Theoptimizer estimates constraint satisfaction probabilities for a candidate action sequence by simulatingit repeatedly over f dyn .
The ﬁrst chance constraint ensures the agent maintains the ability to returnto safe states where it knows how to do the task within H steps if necessary.
Because the agentreplans at each timestep, the agent need not return to the Safe Set: during training, the Safe Setexpands, enabling further exploration.
In practice, we set  S for the Safe Set classiﬁer f S adaptively asdescribed in the supplement.
The second chance constraint encourages constraint violation probabilityof no more than  C .
After solving the optimization problem, the agent executes the ﬁrst action in theplan: ⇡ ( z t ) =
a t where a t is the ﬁrst element of a ⇤ t : t + H  1 , observes a new state, and replans.
Experiments
We evaluate LS 3 on 3 robotic control tasks in simulation and a physical cable routing task on theda Vinci Research Kit (dVRK) [44].
Safe RL is of particular interest for surgical robots such asthe dVRK due to its delicate structure, motivating safety, and relatively imprecise controls [16, 45],motivating closed-loop control.
We study whether LS 3 can learn more safely and efﬁciently thanalgorithms that do not structure exploration based on prior task successes.
5.1 Comparisons
We evaluate LS 3 in comparison to prior algorithms that behavior clone suboptimal demonstrationsbefore exploring online ( SACfD ) [46] or leverage ofﬂine reinforcement learning to learn a policyusing all ofﬂine data before updating the policy online ( AWAC ) [47].
For both of these comparisonswe enforce constraints via a tuned reward penalty of  for constraint violations as in [48].
We alsoimplement a version of SACfD with a learned recovery policy ( SACfD+RRL ) using the Recovery RLalgorithm [35] to use prior constraint violating data to try to avoid constraint violating states.
We thencompare LS 3 to an ablated version without the Safe Set constraint (just binary classiﬁcation (BC)) inequation 5 ( LS 3 (  Safe Set) ) to evaluate if the Safe Set promotes consistent task completion andstable learning.
Finally, we compare LS 3 to a an ablated version of the Safe Set classiﬁer (Section 4.2)without a recursive objective, where the classiﬁer is just trained to predict S j ( LS 3 (BC SS) ).
Seethe supplement for details on hyperparameters and ofﬂine data used for LS 3 and prior algorithms.
5.2 Evaluation Metrics
For each algorithm on each domain, we aggregate statistics over random seeds (10 for simulationexperiments, 3 for the physical experiment), reporting the mean and standard error across the seeds.
We present learning curves that show the total sum reward for each training trajectory to study howefﬁciently LS 3 and the comparisons learn each task.
Because all tasks use the sparse task completionbased rewards deﬁned in Section 3, the total reward for a trajectory is the time to reach the goalset, where more negative rewards correspond to slower convergence to G .
Thus, for a task with taskhorizon T , a total reward greater than  T implies successful task completion.
The state is frozen inplace upon constraint violation until the task horizon elapses.
We report task success and constraintsatisfaction rates for LS 3 and comparisons to study if algorithms consistently complete the taskduring learning and satisfy user-speciﬁed state-space constraints.
LS 3 collects K = 10 trajectories inbetween training phases on simulated tasks and K
= 5 in between training phases for physical tasks,while the SACfD and AWAC comparisons update their parameters after each timestep.
This presentsa calibrated metric in terms of the amount of data collected across algorithms.
5.3 Domains
In simulation, we evaluate LS 3 on 3 vision-based continuous control domains that are illustratedin Figure 3.
We evaluate LS 3 and comparisons on a constrained visual navigation task (Pointmass
Navigation) where the agent navigates from a ﬁxed start state to a ﬁxed goal set while avoiding alarge central obstacle.
We study this domain to gain intuition and visualize the learned value function,goal/constraint indicators, and Safe Set in Figure 2.
We then study a constrained image-basedreaching task (Reacher) based on [49], where the objective is to navigate the end effector of a 2-linkplanar robotic arm to a yellow goal position without the end-effector entering a red stay out zone.
Wethen study a challenging sequential image-based robotic pushing domain (Sequential Pushing), inwhich the objective is to push each of the 3 blocks forward on the table without pushing them to eitherside and causing them to fall out of the workspace.
Finally, we evaluate LS 3 with an image-basedphysical experiment on the da Vinci Research Kit (dVRK) [50] (Figure 3), where the objective is toguide the endpoint of a cable to a goal region without letting the cable or end effector collide with anobstacle.
The Pointmass Navigation and Reaching domains have a task horizon of T = 100 whilethe Sequential Pushing domain and physical experiment have task horizons of T = 150 and T = 50respectively.
See the supplement for more details on all domains.
5.4 Simulation Results
We ﬁnd that LS 3 is able to learn more stably and efﬁciently than all comparisons across all simulateddomains while converging to similar performance within 250 trajectories collected online (Figure 4).
LS 3 is able to consistently complete the task during learning, while the comparisons, which do notlearn a Safe Set to structure exploration based on prior successes, exhibit much less stable learning.
Additionally, in Table 1 and Table 2, we report the task success rate and constraint violation rate ofall algorithms during training.
We ﬁnd that LS 3 achieves a signiﬁcantly higher task success rate thancomparisons on all tasks.
We also ﬁnd that LS 3 violates constraints less often than comparisons on the Reacher task, but violates constraints more often than SACfD and SACfD+RRL on the other domains. This is because SACfD and SACfD+RRL spend much less time in the neighborhood of constraintviolating states during training due to their lower task success rates.
Because they do not efﬁcientlylearn to perform the tasks, they do not violate constraints as often.
We ﬁnd that the AWAC comparisonachieves very low task performance.
While AWAC is designed for ofﬂine reinforcement learning, tothe best of our knowledge, it has not been previously evaluated on long-horizon, image-based tasksas in this paper, which we hypothesize are very challenging for it.
[Table] [Table]
We ﬁnd LS 3 has a lower success rate when the Safe Set constraint is removed (LS 3 (  Safe Set)) as expected.
The Safe Set is particularly important in the sequential pushing task, and LS 3 (  Safe Set)has a much lower task completion rate than LS 3 .
LS 3 without the recursive classiﬁcation objectivefrom Section 4.2 (LS 3 (BC SS)) has similar performance to LS 3 on the navigation environment,but learns substantially more slowly on the Reacher environment and performs signiﬁcantly worsethan LS 3 on the more challenging Pushing environment as the learned Safe Set is unable to exploittemporal structure to distinguish safe states from unsafe states.
See the supplement for details onexperimental parameters and ofﬂine data used for LS 3 and comparisons and ablations studying theeffect of the planning horizon and threshold used to deﬁne the Safe Set.
5.5 Physical Results
In physical experiments, we compare LS 3 to SACfD and SACfD+RRL (Figure 5) on the physical cablerouting task illustrated in Figure 3.
We ﬁnd LS 3 quickly outperforms the suboptimal demonstrationswhile succeeding at the task signiﬁcantly more often than both comparisons, which are unable to learnthe task and also violate constraints more than LS 3 .
We hypothesize that the difﬁculty of reasoningabout cable collisions and deformation from images makes it challenging for prior algorithms tomake sufﬁcient task progress as they do not use prior successes to structure exploration.
See thesupplement for details on experimental parameters and ofﬂine data used for LS 3 and comparisons.
Discussion and Future Work
We present LS 3 , a scalable algorithm for safe and efﬁcient policy learning for visuomotor tasks.
LS 3structures exploration by learning a Safe Set in a learned latent space, which captures the set ofstates from which the agent is conﬁdent in task completion.
LS 3 then ensures that the agent can planback to states in the Safe Set, encouraging consistent task completion during learning.
Experimentssuggest that LS 3 can safely and efﬁciently learn 4 visuomotor control tasks, including a challengingsequential pushing task in simulation and a cable routing task on a physical robot.
In future work, weare excited to explore further physical evaluation of LS 3 on safety critical visuomotor control tasksand applications to systems with dynamic constraints on velocity or acceleration.