In robotics, an RL policy that maximizes expected return but occasionally violates physical limits (joint torque bounds, collision distances, thermal limits, battery constraints, etc.) is unacceptable. We therefore replace the unconstrained optimization objective \( \max_{\pi} J_r(\pi) \) with a constrained problem that enforces explicit safety signals. Typical constraint costs encode:

• Collisions or near-collisions with the environment or self.
• Violations of torque/velocity/acceleration bounds.
• Thermal or power usage exceeding safe operating regions.

We assume students already understand continuous-control MDPs and policy gradient methods from earlier lessons. Here we add a set of cost signals and thresholds. At a high level, the safety-aware RL loop in robotics looks like:

flowchart TD
  A["Robot + environment"] --> B["Specify reward r(s,a)"]
  A --> C["Specify safety costs c_i(s,a) and limits d_i"]
  B --> D["Formulate CMDP"]
  C --> D
  D --> E["Choose safe RL algorithm (Lagrangian, primal-dual, risk-sensitive)"]
  E --> F["Train policy with safety layer or safe exploration"]
  F --> G["Validate safety on sim and hardware"]
  G --> H["Deploy with monitoring and fallbacks"]
        

The rest of the lesson makes this pipeline mathematically precise, and shows how to implement core building blocks in software.

A constrained Markov decision process augments a standard MDP with m constraint cost signals and corresponding limits. Formally, for continuous-control robotics we consider

\[ \mathcal{M}_c = (\mathcal{S},\mathcal{A},P,r,\{c_i\}_{i=1}^m,\gamma,\mu), \]

where \( \mathcal{S} \) is the state space (joint angles, velocities, contact indicators, etc.), \( \mathcal{A} \) the action space (torques, velocities), \( P \) the transition kernel, \( r:\mathcal{S}\times\mathcal{A}\to\mathbb{R} \) the scalar reward, \( c_i:\mathcal{S}\times\mathcal{A}\to\mathbb{R}_+ \) constraint costs, \( \gamma\in[0,1) \) a discount factor, and \( \mu \) an initial state distribution.

For a stationary policy \( \pi(a\mid s) \), the usual discounted return is

\[ J_r(\pi) = \mathbb{E}_{\pi}\bigg[\sum_{t=0}^{\infty} \gamma^t r(s_t,a_t)\bigg], \]

and the constraint returns are

\[ J_{c_i}(\pi) = \mathbb{E}_{\pi}\bigg[\sum_{t=0}^{\infty} \gamma^t c_i(s_t,a_t)\bigg], \quad i=1,\dots,m. \]

A safety-constrained RL problem is the CMDP optimization

\[ \begin{aligned} \max_{\pi} \quad & J_r(\pi) \\ \text{s.t.} \quad & J_{c_i}(\pi) \leq d_i,\quad i=1,\dots,m, \end{aligned} \]

where each \( d_i \) is a design parameter expressing the maximum allowable discounted frequency or magnitude of violations. For example, \( c_1 \) could be an indicator of collision events, so \( J_{c_1}(\pi) \) bounds the discounted collision probability.

In finite CMDPs one can equivalently optimize over occupancy measures \( d_{\pi}(s,a) \) which satisfy linear balance constraints. Let

\[ d_{\pi}(s,a) = (1-\gamma) \sum_{t=0}^{\infty} \gamma^t \mathbb{P}_{\pi}(s_t=s,a_t=a), \]

then we can write

\[ \begin{aligned} J_r(\pi) &= \frac{1}{1-\gamma} \sum_{s,a} d_{\pi}(s,a)\,r(s,a), \\ J_{c_i}(\pi) &= \frac{1}{1-\gamma} \sum_{s,a} d_{\pi}(s,a)\,c_i(s,a). \end{aligned} \]

This makes the CMDP optimization a linear program in \( d_{\pi} \). Deep RL algorithms can be viewed as approximate stochastic methods for this LP, but implemented in policy parameter space.

A standard approach to solving CMDPs is to form the Lagrangian by relaxing the inequality constraints with nonnegative multipliers \( \lambda_i \geq 0 \). Define

\[ \mathcal{L}(\pi,\boldsymbol{\lambda}) = J_r(\pi) - \sum_{i=1}^m \lambda_i \big(J_{c_i}(\pi) - d_i\big), \quad \lambda_i \geq 0. \]

The constrained problem is equivalent (under standard CMDP regularity assumptions) to the saddle-point problem

\[ \max_{\pi} \min_{\boldsymbol{\lambda} \geq \mathbf{0}} \mathcal{L}(\pi,\boldsymbol{\lambda}). \]

We parameterize policies \( \pi_{\theta} \) with parameters \( \theta \in \mathbb{R}^d \) (e.g., a neural network policy). Let the reward return from time \( t \) be

\[ G_t^r = \sum_{k=t}^{\infty} \gamma^{k-t} r(s_k,a_k), \]

and similarly the constraint returns \( G_t^{c_i} \). Using the log-derivative trick, the reward policy gradient is

\[ \nabla_{\theta} J_r(\pi_{\theta}) = \mathbb{E}_{\pi_{\theta}}\bigg[ \sum_{t=0}^{\infty} \nabla_{\theta} \log \pi_{\theta}(a_t \mid s_t)\, G_t^r \bigg]. \]

Exactly the same derivation shows

\[ \nabla_{\theta} J_{c_i}(\pi_{\theta}) = \mathbb{E}_{\pi_{\theta}}\bigg[ \sum_{t=0}^{\infty} \nabla_{\theta} \log \pi_{\theta}(a_t \mid s_t)\, G_t^{c_i} \bigg]. \]

Therefore the Lagrangian gradient with respect to \( \theta \) is

\[ \nabla_{\theta} \mathcal{L}(\pi_{\theta},\boldsymbol{\lambda}) = \nabla_{\theta} J_r(\pi_{\theta}) - \sum_{i=1}^m \lambda_i \nabla_{\theta} J_{c_i}(\pi_{\theta}). \]

In practice we approximate these expectations by Monte Carlo estimates from rollouts. For a batch of trajectories indexed by \( b \), an unbiased estimator is

\[ \widehat{\nabla_{\theta} \mathcal{L}} = \frac{1}{B} \sum_{b=1}^B \sum_{t=0}^{T_b-1} \nabla_{\theta}\log\pi_{\theta}(a_t^{(b)} \mid s_t^{(b)}) \bigg( \hat{G}_{t}^{r,(b)} - \sum_{i=1}^m \lambda_i \hat{G}_{t}^{c_i,(b)} \bigg), \]

where \( \hat{G} \) are possibly baseline-adjusted returns.

Primal–dual updates. A simple stochastic primal–dual algorithm alternates between:

\[ \begin{aligned} \theta_{k+1} &= \theta_k + \eta_{\theta}\, \widehat{\nabla_{\theta} \mathcal{L}(\pi_{\theta_k},\boldsymbol{\lambda}_k)}, \\ \lambda_{i,k+1} &= \Big[\lambda_{i,k} + \eta_{\lambda}\, \big(\widehat{J_{c_i}(\pi_{\theta_k})} - d_i\big)\Big]_+, \quad i=1,\dots,m, \end{aligned} \]

where \( [x]_+ = \max(x,0) \). The multiplier update is a stochastic gradient ascent step on the dual, pushing \( \lambda_i \) upward whenever the corresponding constraint is violated on average.

Under suitable assumptions (bounded gradients, diminishing step sizes, CMDP convexity in occupancy-measure space), one can show that these updates converge to a saddle point \( (\pi^{\star}, \boldsymbol{\lambda}^{\star}) \), and that \( \pi^{\star} \) satisfies the constraints and is optimal among safe policies. In practice, deep neural parameterizations and finite data break convexity, but the same updates are widely used and successful in robotics.

A core difficulty in robotic safety is exploration: naive RL may try obviously unsafe actions before learning they are bad. A common strategy is to start from a known safe baseline policy \( \pi_{\text{safe}} \) (for example a hand-designed controller) and interpolate toward a learned policy \( \pi_{\theta} \).

Consider the mixed policy

\[ \pi_{\alpha}(a \mid s) = (1-\alpha)\,\pi_{\text{safe}}(a\mid s) + \alpha\,\pi_{\theta}(a\mid s), \quad \alpha \in [0,1]. \]

Assuming we randomly choose at the beginning of each episode whether to follow \( \pi_{\text{safe}} \) or \( \pi_{\theta} \) with probabilities \( 1-\alpha \) and \( \alpha \), linearity of expectation yields

\[ J_c(\pi_{\alpha}) = (1-\alpha)\,J_c(\pi_{\text{safe}}) + \alpha\,J_c(\pi_{\theta}) \]

for any constraint cost \( c \). Suppose \( \pi_{\text{safe}} \) is strictly safe: \( J_c(\pi_{\text{safe}}) \leq d - \delta \) for some margin \( \delta > 0 \). If the unconstrained learner proposes a policy \( \pi_{\theta} \) with \( J_c(\pi_{\theta}) > d \), we can still ensure the mixture is safe by choosing \( \alpha \) small enough.

Requiring \( J_c(\pi_{\alpha}) \leq d \) and solving for \( \alpha \) gives:

\[ \begin{aligned} (1-\alpha)J_c(\pi_{\text{safe}}) + \alpha J_c(\pi_{\theta}) &\leq d \\ \alpha \big(J_c(\pi_{\theta}) - J_c(\pi_{\text{safe}})\big) &\leq d - J_c(\pi_{\text{safe}}), \end{aligned} \]

so whenever

\[ \alpha \leq \frac{d - J_c(\pi_{\text{safe}})} {J_c(\pi_{\theta}) - J_c(\pi_{\text{safe}})}, \]

the mixed policy is safe. This simple convex-combination argument underlies many shields and backup policies in safe RL: a safety layer can dynamically reduce \( \alpha \) or switch back to the baseline when estimated constraint values become too large.

flowchart TD
  S["Baseline safe policy pi_safe"] --> M["Proposed RL policy pi_theta"]
  S --> C["Mix policies pi_alpha with alpha in [0,1]"]
  M --> C
  C --> E["Estimate constraint return J_c(pi_alpha)"]
  E --> D1{"Is J_c(pi_alpha) <= d?"}
  D1 -->|yes| USE["Execute mixed policy on robot"]
  D1 -->|no| BACK["Reduce alpha or fall back to pi_safe"]
        

In robotics, \( \pi_{\text{safe}} \) might be a low-gain impedance controller that keeps the arm away from obstacles, while \( \pi_{\theta} \) is a high-performance learned manipulator policy. The mixture allows gradual, guaranteed-safe performance improvements.

CMDPs constrain expected constraint returns. But in robotics it is often more natural to constrain the probability of rare but catastrophic failures, or to control a coherent risk measure of a random cost functional. Let \( Z \) denote the total episodic safety cost (e.g., sum of collision penalties).

A chance constraint limits the probability of large costs:

\[ \mathbb{P}_{\pi}\big(Z > \bar{c}\big) \leq \delta, \]

where \( \bar{c} \) is an unacceptable cost level and \( \delta \) a small probability. This is challenging to handle directly, but upper bounds (e.g., Markov or Chernoff bounds) can convert it into CMDP-like expectations.

A more refined approach uses the Conditional Value at Risk (CVaR), which looks at the tail of the cost distribution. For confidence level \( \alpha\in(0,1) \), the CVaR of \( Z \) is

\[ \text{CVaR}_{\alpha}(Z) = \inf_{\eta \in \mathbb{R}} \bigg\{\eta + \frac{1}{1-\alpha} \mathbb{E}\big[(Z-\eta)_+\big]\bigg\}. \]

Here \( (x)_+ = \max(x,0) \). A risk-sensitive safe RL problem is then

\[ \begin{aligned} \min_{\pi} \quad & \text{CVaR}_{\alpha}(Z_{\pi}) \\ \text{s.t.} \quad & J_r(\pi) \geq \rho, \end{aligned} \]

or equivalently maximize return subject to \( \text{CVaR}_{\alpha}(Z_{\pi}) \leq \bar{z} \). Using the infimum representation, one can introduce an auxiliary variable \( \eta \) and perform stochastic gradient descent over \( (\theta,\eta) \) with unbiased gradient estimators derived from samples of \( Z \).

For robotics, risk-sensitive formulations reduce the frequency of catastrophic failures (e.g., dropping objects, hitting humans) even if the expected cost is already small. They complement CMDP-style constraints and can be combined with Lagrangian methods.

We sketch a minimal Python implementation of a primal–dual constrained policy gradient for a simple continuous robot (e.g., a 1D position control task). We assume familiarity with PyTorch and gymnasium from earlier labs. The constraint cost here could be, for example, a penalty whenever the robot's position leaves a safe interval.

import torch
import torch.nn as nn
import torch.optim as optim
import gymnasium as gym

device = torch.device("cuda" if torch.cuda.is_available() else "cpu")

env = gym.make("Pendulum-v1")  # stand-in for a torque-limited joint
obs_dim = env.observation_space.shape[0]
act_dim = env.action_space.shape[0]

# --- Actor-Critic with separate value heads for reward and constraint ---

class ActorCritic(nn.Module):
    def __init__(self, obs_dim, act_dim):
        super().__init__()
        self.body = nn.Sequential(
            nn.Linear(obs_dim, 64), nn.Tanh(),
            nn.Linear(64, 64), nn.Tanh(),
        )
        self.mu_head = nn.Linear(64, act_dim)
        self.log_std = nn.Parameter(torch.zeros(act_dim))

        self.v_r = nn.Linear(64, 1)   # reward value
        self.v_c = nn.Linear(64, 1)   # constraint value

    def forward(self, obs):
        x = self.body(obs)
        mu = self.mu_head(x)
        std = self.log_std.exp()
        v_r = self.v_r(x)
        v_c = self.v_c(x)
        return mu, std, v_r, v_c

    def act(self, obs):
        obs_t = torch.as_tensor(obs, dtype=torch.float32, device=device)
        mu, std, _, _ = self.forward(obs_t)
        dist = torch.distributions.Normal(mu, std)
        a = dist.sample()
        logp = dist.log_prob(a).sum(-1)
        return a.cpu().numpy(), logp

net = ActorCritic(obs_dim, act_dim).to(device)
optimizer = optim.Adam(net.parameters(), lr=3e-4)

lambda_c = torch.tensor(0.0, device=device, requires_grad=False)
lambda_lr = 1e-2
gamma = 0.99
cost_limit = 1.0

def compute_returns(rewards, costs, gamma):
    G_r, G_c = [], []
    g_r = 0.0
    g_c = 0.0
    for r, c in zip(reversed(rewards), reversed(costs)):
        g_r = r + gamma * g_r
        g_c = c + gamma * g_c
        G_r.append(g_r)
        G_c.append(g_c)
    G_r.reverse()
    G_c.reverse()
    return torch.tensor(G_r, dtype=torch.float32, device=device), \
           torch.tensor(G_c, dtype=torch.float32, device=device)

for epoch in range(1000):
    logps = []
    G_r_list = []
    G_c_list = []
    v_r_list = []
    v_c_list = []
    episode_returns = []
    episode_costs = []

    for _ in range(10):  # batch of episodes
        obs, _ = env.reset()
        done = False
        rewards = []
        costs = []
        logp_traj = []
        vals_r = []
        vals_c = []
        while not done:
            # define simple safety cost: penalty if position outside safe band
            x = obs[0]
            cost = 1.0 if abs(x) > 1.0 else 0.0

            action, logp = net.act(obs)
            next_obs, reward, terminated, truncated, _ = env.step(action)
            done = terminated or truncated

            with torch.no_grad():
                obs_t = torch.as_tensor(obs, dtype=torch.float32, device=device)
                _, _, v_r, v_c = net(obs_t.unsqueeze(0))
            vals_r.append(v_r.squeeze(0))
            vals_c.append(v_c.squeeze(0))

            rewards.append(float(reward))
            costs.append(float(cost))
            logp_traj.append(logp)
            obs = next_obs

        G_r, G_c = compute_returns(rewards, costs, gamma)
        logps.append(torch.stack(logp_traj))
        G_r_list.append(G_r)
        G_c_list.append(G_c)
        v_r_list.append(torch.stack(vals_r))
        v_c_list.append(torch.stack(vals_c))
        episode_returns.append(sum(rewards))
        episode_costs.append(sum(costs))

    logps = torch.cat(logps)
    G_r_list = torch.cat(G_r_list)
    G_c_list = torch.cat(G_c_list)
    v_r_list = torch.cat(v_r_list)
    v_c_list = torch.cat(v_c_list)

    # Advantage estimates (simple baseline)
    adv_r = G_r_list - v_r_list.detach()
    adv_c = G_c_list - v_c_list.detach()

    # Policy loss: maximize L = E[adv_r - lambda_c * adv_c]
    # (gradient ascent → minimize negative)
    loss_policy = -(logps * (adv_r - lambda_c * adv_c)).mean()

    # Value losses
    loss_v_r = 0.5 * (G_r_list - v_r_list).pow(2).mean()
    loss_v_c = 0.5 * (G_c_list - v_c_list).pow(2).mean()

    loss = loss_policy + loss_v_r + loss_v_c

    optimizer.zero_grad()
    loss.backward()
    optimizer.step()

    # Dual update: push lambda_c up when cost exceeds limit
    avg_episode_cost = sum(episode_costs) / len(episode_costs)
    lambda_c = torch.clamp(lambda_c + lambda_lr *
                           (avg_episode_cost - cost_limit), min=0.0)

    print(f"Epoch {epoch}, return {sum(episode_returns)/len(episode_returns):.2f}, "
          f"avg cost {avg_episode_cost:.2f}, lambda {lambda_c.item():.3f}")
      

In a realistic robotic setup, cost would be computed from distances to obstacles (via a geometry library or a simulator like MuJoCo), and the environment would be a high-dimensional manipulator or mobile robot task. The same primal–dual updates apply.

In C++ one typically integrates safe RL with ROS2 for real robots. Below is a highly simplified skeleton illustrating how one might structure an actor-critic with a Lagrange multiplier in C++ using Eigen for linear algebra. The robot dynamics and simulator interface would be provided by ROS2 and physics engines (e.g., Gazebo, Isaac Sim, Drake).

#include <rclcpp/rclcpp.hpp>
#include <Eigen/Dense>
#include <random>

using Eigen::VectorXd;
using Eigen::MatrixXd;

class SafeActorCritic {
public:
    SafeActorCritic(int obs_dim, int act_dim)
    : obs_dim_(obs_dim), act_dim_(act_dim),
      lambda_c_(0.0), lambda_lr_(1e-3)
    {
        // Initialize parameters (e.g., small random values)
        theta_ = VectorXd::Zero(num_params());
        w_r_ = VectorXd::Zero(num_value_params());
        w_c_ = VectorXd::Zero(num_value_params());
    }

    VectorXd act(const VectorXd &obs, double &logp_out) {
        // Gaussian policy: a = mu_theta(obs) + sigma * noise
        VectorXd phi = feature_actor(obs);
        VectorXd mu = W_policy_ * phi;
        double sigma = 0.1;  // fixed std for illustration

        VectorXd noise = VectorXd::NullaryExpr(act_dim_, [&](){
            return normal_(rng_);
        });
        VectorXd a = mu + sigma * noise;

        double logp = -0.5 * (noise.squaredNorm()
                     + act_dim_ * std::log(2.0 * M_PI * sigma * sigma));
        logp_out = logp;
        return a;
    }

    void update_batch(const std::vector<VectorXd> &obs_traj,
                      const std::vector<VectorXd> &act_traj,
                      const std::vector<double> &rew_traj,
                      const std::vector<double> &cost_traj,
                      double gamma)
    {
        // Compute discounted returns G_r, G_c
        int T = static_cast<int>(rew_traj.size());
        std::vector<double> G_r(T), G_c(T);
        double g_r = 0.0, g_c = 0.0;
        for (int t = T - 1; t >= 0; --t) {
            g_r = rew_traj[t] + gamma * g_r;
            g_c = cost_traj[t] + gamma * g_c;
            G_r[t] = g_r;
            G_c[t] = g_c;
        }

        // Simple gradient estimates (omitting baselines for brevity)
        VectorXd grad_theta = VectorXd::Zero(theta_.size());
        double avg_cost = 0.0;
        for (int t = 0; t < T; ++t) {
            VectorXd phi = feature_actor(obs_traj[t]);
            VectorXd mu = W_policy_ * phi;
            // score function grad wrt theta (here compressed as grad_mu)
            VectorXd grad_mu = phi;  // for linear policy mu = W * phi
            VectorXd a = act_traj[t];
            VectorXd diff = a - mu;
            double sigma = 0.1;

            double factor = (G_r[t] - lambda_c_ * G_c[t]) / (sigma * sigma);
            grad_theta += factor * kron(diff, grad_mu); // kron product

            avg_cost += cost_traj[t];
        }
        avg_cost /= static_cast<double>(T);

        // Gradient ascent on theta
        theta_ += 1e-3 * grad_theta;

        // Dual update
        lambda_c_ = std::max(0.0, lambda_c_ + lambda_lr_ *
                                       (avg_cost - cost_limit_));
    }

private:
    int obs_dim_, act_dim_;
    VectorXd theta_, w_r_, w_c_;
    MatrixXd W_policy_;
    double lambda_c_;
    double lambda_lr_;
    double cost_limit_ = 1.0;

    std::mt19937 rng_{std::random_device{}()};
    std::normal_distribution<double> normal_{0.0, 1.0};

    int num_params() const { return obs_dim_ * act_dim_; }
    int num_value_params() const { return obs_dim_; }

    VectorXd feature_actor(const VectorXd &obs) const {
        // e.g., identity features
        return obs;
    }

    VectorXd kron(const VectorXd &a, const VectorXd &b) const {
        VectorXd out(a.size() * b.size());
        int k = 0;
        for (int i = 0; i < a.size(); ++i) {
            for (int j = 0; j < b.size(); ++j) {
                out(k++) = a(i) * b(j);
            }
        }
        return out;
    }
};
      

In a ROS2 node, one would wrap SafeActorCritic inside a rclcpp::Node, subscribe to joint state topics, publish control commands, and periodically run update_batch based on experience buffers collected in simulation or on hardware.

In Java, safe RL can be implemented using libraries like ND4J and RL4J (part of DeepLearning4J), or custom environments interfaced to robotics middleware (e.g., ROSJava). The following skeleton illustrates a constrained agent interface.

public interface SafeEnvironment {
    double[] reset();
    StepResult step(double[] action);
    int getObservationDim();
    int getActionDim();
    // StepResult contains obs, reward, done, constraintCost
}

public class SafeRLAgent {
    private int obsDim;
    private int actDim;
    private double lambdaC = 0.0;
    private double lambdaLr = 1e-2;
    private double costLimit = 1.0;

    // Parameters of policy and value functions would live here
    // (e.g., INDArray weights if using ND4J).

    public SafeRLAgent(int obsDim, int actDim) {
        this.obsDim = obsDim;
        this.actDim = actDim;
    }

    public double[] act(double[] obs) {
        // Sample from Gaussian policy (stochastic exploration).
        double[] mu = policyMean(obs);
        double[] std = policyStd(obs);
        double[] action = new double[actDim];
        for (int i = 0; i < actDim; i++) {
            action[i] = mu[i] + std[i] * randomNormal();
        }
        return action;
    }

    public void updateOnTrajectory(List<double[]> obsTraj,
                                   List<double[]> actTraj,
                                   List<Double> rewTraj,
                                   List<Double> costTraj,
                                   double gamma)
    {
        int T = rewTraj.size();
        double[] G_r = new double[T];
        double[] G_c = new double[T];
        double gR = 0.0, gC = 0.0;
        for (int t = T - 1; t >= 0; t--) {
            gR = rewTraj.get(t) + gamma * gR;
            gC = costTraj.get(t) + gamma * gC;
            G_r[t] = gR;
            G_c[t] = gC;
        }

        // Compute gradients and update policy parameters here.
        // For brevity we omit ND4J tensor operations.

        double avgCost = 0.0;
        for (double c : costTraj) {
            avgCost += c;
        }
        avgCost /= (double) T;

        // Dual update
        lambdaC = Math.max(0.0, lambdaC + lambdaLr * (avgCost - costLimit));
    }

    private double[] policyMean(double[] obs) {
        // Placeholder linear policy
        double[] mu = new double[actDim];
        for (int i = 0; i < actDim; i++) {
            mu[i] = 0.0;
        }
        return mu;
    }

    private double[] policyStd(double[] obs) {
        double[] std = new double[actDim];
        for (int i = 0; i < actDim; i++) {
            std[i] = 0.2; // fixed std
        }
        return std;
    }

    private double randomNormal() {
        return new java.util.Random().nextGaussian();
    }
}
      

A concrete implementation would add neural-network policy/value functions, replay buffers, and integration with a robotic simulator or real-time control loop.

MATLAB provides both Reinforcement Learning Toolbox and Robotics System Toolbox, making it natural to prototype safety-constrained algorithms. The following script shows a discrete safe Q-learning prototype where a constraint cost is added and a Lagrange multiplier penalizes unsafe behavior. In Simulink, the same logic can be embedded in an RL Agent block connected to a manipulator model.

gamma = 0.99;
alpha = 0.1;      % Q-learning step size
lambda_lr = 1e-2;
lambda_c = 0.0;
cost_limit = 0.5;

nStates = 20;
nActions = 3;

Q_r = zeros(nStates, nActions);  % reward Q
Q_c = zeros(nStates, nActions);  % constraint Q

env = MySafeRobotEnv();  % user-defined MATLAB class

for episode = 1:1000
    s = reset(env);  % state index
    done = false;
    ep_cost = 0.0;

    while ~done
        % epsilon-greedy action selection
        if rand() < 0.1
            a = randi(nActions);
        else
            [~, a] = max(Q_r(s,:) - lambda_c * Q_c(s,:));
        end

        [s_next, r, c, done] = step(env, a);
        ep_cost = ep_cost + c;

        % standard Q-learning updates
        best_next = max(Q_r(s_next,:) - lambda_c * Q_c(s_next,:));

        td_target_r = r + gamma * best_next;
        td_error_r = td_target_r - Q_r(s,a);
        Q_r(s,a) = Q_r(s,a) + alpha * td_error_r;

        td_target_c = c + gamma * max(Q_c(s_next,:));
        td_error_c = td_target_c - Q_c(s,a);
        Q_c(s,a) = Q_c(s,a) + alpha * td_error_c;

        s = s_next;
    end

    % Dual variable update at end of episode
    lambda_c = max(0.0, lambda_c + lambda_lr * (ep_cost - cost_limit));

    fprintf("Episode %d, cost %.3f, lambda %.3f\n", ...
            episode, ep_cost, lambda_c);
end
      

In Simulink, the environment MySafeRobotEnv would wrap a joint-space manipulator model. The constraint signal c can be derived from joint limit violations or minimum distance to obstacles, implemented via blocks from Robotics System Toolbox.

For small finite CMDPs, safety-constrained RL reduces to a linear program in the occupancy measures. Wolfram Mathematica is well suited for such symbolic and numeric optimization. The code below solves a toy CMDP with two states and two actions.

(* States and actions *)
states = {1, 2};
actions = {1, 2};
gamma = 0.9;

(* Reward and cost tables r(s,a), c(s,a) *)
r[1,1] = 1;  r[1,2] = 0;
r[2,1] = 0;  r[2,2] = 2;

c[1,1] = 0;  c[1,2] = 1;
c[2,1] = 1;  c[2,2] = 0;

(* Transition probabilities P(s'|s,a) *)
P[1,1,1] = 0.7; P[2,1,1] = 0.3;
P[1,1,2] = 0.2; P[2,1,2] = 0.8;
P[1,2,1] = 0.4; P[2,2,1] = 0.6;
P[1,2,2] = 0.9; P[2,2,2] = 0.1;

(* Decision variables: discounted occupancy measures d[s,a] *)
vars = Flatten@Table[d[s,a], {s,states}, {a,actions}];

(* Initial distribution mu(s) *)
mu[1] = 1; mu[2] = 0;

(* Balance constraints:
   sum_a d[s,a] = (1 - gamma) * mu(s) +
   gamma * sum_{s',a'} d[s',a'] P(s|s',a') *)
balanceConstraints =
  Table[
    Sum[d[s,a], {a,actions}] == (1 - gamma) * mu[s] +
      gamma * Sum[d[s2,a2] * P[s,s2,a2],
                  {s2,states}, {a2,actions}],
    {s,states}
  ];

(* Nonnegativity *)
nonneg = Thread[vars >= 0];

(* Cost constraint: J_c(d) <= d_max *)
dMax = 0.5;
Jc = 1/(1 - gamma) * Sum[d[s,a] * c[s,a],
                         {s,states}, {a,actions}];
costConstraint = Jc <= dMax;

(* Objective: maximize discounted reward *)
Jr = 1/(1 - gamma) * Sum[d[s,a] * r[s,a],
                         {s,states}, {a,actions}];

sol = NMaximize[
   {Jr, Join[balanceConstraints, nonneg, {costConstraint}]},
   vars
];

sol
      

The resulting optimal occupancy measures define an optimal safe policy. This explicit LP formulation is intractable for large robotic systems, but it is extremely useful for theoretical analysis and for validating approximate RL algorithms on tiny benchmarks.

Problem 1 (Occupancy-Measure LP for CMDPs): Consider a finite CMDP with state set \( \mathcal{S} \), action set \( \mathcal{A} \), transition kernel \( P \), reward \( r \), single constraint cost \( c \), discount \( \gamma\in[0,1) \), and initial distribution \( \mu \). Show that the constrained problem

\[ \max_{\pi} J_r(\pi) \quad \text{s.t.}\quad J_c(\pi) \leq d \]

can be written as a linear program over discounted occupancy measures \( d(s,a) \).

Solution: For a stationary policy \( \pi \), define discounted occupancy measures

\[ d(s,a) = (1-\gamma) \sum_{t=0}^{\infty} \gamma^t \mathbb{P}_{\pi}(s_t=s,a_t=a). \]

It follows that \( d(s,a) \geq 0 \) and the state marginals satisfy the balance equations

\[ \sum_{a} d(s,a) = (1-\gamma)\mu(s) + \gamma \sum_{s',a'} d(s',a') P(s\mid s',a'). \]

Using the definition of \( d \), we obtain

\[ \begin{aligned} J_r(\pi) &= \mathbb{E}_{\pi}\bigg[ \sum_{t=0}^{\infty} \gamma^t r(s_t,a_t)\bigg] = \frac{1}{1-\gamma} \sum_{s,a} d(s,a)\,r(s,a), \\ J_c(\pi) &= \frac{1}{1-\gamma} \sum_{s,a} d(s,a)\,c(s,a). \end{aligned} \]

Thus the CMDP is equivalent to

\[ \begin{aligned} \max_{d} \quad & \sum_{s,a} d(s,a)\,r(s,a) \\ \text{s.t.}\quad & \sum_{a} d(s,a) = (1-\gamma)\mu(s) + \gamma \sum_{s',a'} d(s',a') P(s\mid s',a'),\quad \forall s, \\ & \sum_{s,a} d(s,a)\,c(s,a) \leq (1-\gamma)d, \\ & d(s,a) \geq 0,\quad \forall s,a, \end{aligned} \]

which is a linear program in the variables \( d(s,a) \).

Problem 2 (Unbiased Gradient Estimator for Constraint Returns): Let \( J_c(\pi_{\theta}) \) be the discounted constraint return for a differentiable policy \( \pi_{\theta} \). Show that

\[ \widehat{\nabla_{\theta} J_c} = \sum_{t=0}^{T-1} \nabla_{\theta} \log\pi_{\theta}(a_t\mid s_t)\, \hat{G}_t^c \]

is an unbiased estimator of \( \nabla_{\theta} J_c(\pi_{\theta}) \) when \( \hat{G}_t^c \) is the Monte Carlo return \( \sum_{k=t}^{\infty} \gamma^{k-t} c(s_k,a_k) \).

Solution: Starting from

\[ J_c(\pi_{\theta}) = \sum_{\tau} p_{\theta}(\tau) \bigg(\sum_{t=0}^{\infty} \gamma^t c(s_t,a_t)\bigg), \]

where \( \tau \) is a trajectory and \( p_{\theta}(\tau) \) its probability, differentiate under the sum:

\[ \begin{aligned} \nabla_{\theta} J_c(\pi_{\theta}) &= \sum_{\tau} \nabla_{\theta} p_{\theta}(\tau) \sum_{t=0}^{\infty} \gamma^t c(s_t,a_t) \\ &= \sum_{\tau} p_{\theta}(\tau) \nabla_{\theta} \log p_{\theta}(\tau) \sum_{t=0}^{\infty} \gamma^t c(s_t,a_t). \end{aligned} \]

Since \( \log p_{\theta}(\tau) = \sum_{t} \log\pi_{\theta}(a_t\mid s_t) \),

\[ \nabla_{\theta} \log p_{\theta}(\tau) = \sum_{t=0}^{\infty} \nabla_{\theta}\log\pi_{\theta}(a_t\mid s_t). \]

Reordering sums and using the definition of \( G_t^c \) yields

\[ \nabla_{\theta} J_c(\pi_{\theta}) = \mathbb{E}_{\pi_{\theta}}\bigg[ \sum_{t=0}^{\infty} \nabla_{\theta}\log\pi_{\theta}(a_t\mid s_t)\, G_t^c\bigg], \]

so replacing the expectation with a finite-sample average over trajectories gives an unbiased estimator.

Problem 3 (Mixture Safety Bound): Let \( \pi_{\text{safe}} \) and \( \pi_{\theta} \) be policies with constraint returns \( J_c(\pi_{\text{safe}}) \) and \( J_c(\pi_{\theta}) \). Assume \( J_c(\pi_{\text{safe}}) \leq d \) and \( J_c(\pi_{\theta}) > d \). Derive the maximal mixing coefficient \( \alpha_{\max} \) such that \( \pi_{\alpha} = (1-\alpha)\pi_{\text{safe}} + \alpha \pi_{\theta} \) still satisfies \( J_c(\pi_{\alpha}) \leq d \).

Solution: From Section 4 we have

\[ J_c(\pi_{\alpha}) = (1-\alpha)\,J_c(\pi_{\text{safe}}) + \alpha\,J_c(\pi_{\theta}). \]

Requiring \( J_c(\pi_{\alpha}) \leq d \) gives

\[ (1-\alpha)\,J_c(\pi_{\text{safe}}) + \alpha\,J_c(\pi_{\theta}) \leq d. \]

Rearranging,

\[ \alpha \big(J_c(\pi_{\theta}) - J_c(\pi_{\text{safe}})\big) \leq d - J_c(\pi_{\text{safe}}). \]

Since \( J_c(\pi_{\theta}) > J_c(\pi_{\text{safe}}) \), the denominator is positive and

\[ \alpha \leq \frac{d - J_c(\pi_{\text{safe}})} {J_c(\pi_{\theta}) - J_c(\pi_{\text{safe}})} \equiv \alpha_{\max}. \]

Any \( \alpha \in [0,\alpha_{\max}] \) yields a safe mixture.

Problem 4 (CVaR Representation): Let \( Z \) be an integrable random variable. Show that the CVaR representation

\[ \text{CVaR}_{\alpha}(Z) = \inf_{\eta \in \mathbb{R}} \bigg\{\eta + \frac{1}{1-\alpha} \mathbb{E}\big[(Z-\eta)_+\big]\bigg\} \]

is convex in \( Z \). Why is this property important for safe RL?

Solution: For any fixed \( \eta \), the mapping \( Z \mapsto \mathbb{E}[(Z-\eta)_+] \) is convex because \( x \mapsto (x-\eta)_+ \) is convex and expectation preserves convexity. The scaled and shifted version

\[ Z \mapsto \eta + \frac{1}{1-\alpha} \mathbb{E}\big[(Z-\eta)_+\big] \]

is therefore convex for each \( \eta \). The infimum over a family of convex functions is also convex, so \( \text{CVaR}_{\alpha}(Z) \) is convex in \( Z \). For safe RL, convexity allows one to apply stochastic-gradient and primal–dual methods with convergence guarantees in risk-sensitive optimization problems.

Problem 5 (KKT Conditions for CMDP Lagrangian): Consider the CMDP Lagrangian

\[ \mathcal{L}(\pi,\lambda) = J_r(\pi) - \lambda \big(J_c(\pi) - d\big),\quad \lambda \geq 0. \]

Assuming strong duality and convexity in occupancy-measure space, express the Karush–Kuhn–Tucker (KKT) conditions at an optimal pair \( (\pi^{\star},\lambda^{\star}) \) and interpret them in terms of safety.

Solution: The KKT conditions are:

• Primal feasibility: \( J_c(\pi^{\star}) \leq d \).
• Dual feasibility: \( \lambda^{\star} \geq 0 \).
• Complementary slackness: \( \lambda^{\star}\big(J_c(\pi^{\star}) - d\big) = 0 \).
• Stationarity: \( \nabla_{\pi} \mathcal{L}(\pi^{\star},\lambda^{\star}) = 0 \) in the feasible policy manifold.

Complementary slackness implies that if the constraint is inactive (\( J_c(\pi^{\star}) < d \)), then \( \lambda^{\star} = 0 \), so the solution coincides with the unconstrained optimum. If the constraint is active (\( J_c(\pi^{\star}) = d \)), then \( \lambda^{\star} > 0 \), meaning the dual variable acts as a penalty weight enforcing safety at the optimum.

In this lesson we formalized safety-constrained reinforcement learning for robotic systems via constrained Markov decision processes. We expressed CMDPs as linear programs over occupancy measures, derived Lagrangian and primal–dual policy gradient updates that incorporate constraint costs, and analyzed safe exploration strategies based on mixtures with baseline policies. We also introduced risk-sensitive objectives such as CVaR and chance constraints, which better capture rare catastrophic events than expectations alone. Multi-language code sketches in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica illustrated how theoretical ideas map into practical implementations suitable for real robots. These tools provide the foundation for safety-aware RL in advanced robotic manipulation and locomotion tasks.