Struct Barrier

Inheritance Relationships

Derived Type

Struct Documentation

struct Barrier

Abstract base class for Control Barrier Functions.

Barriers enforce safety constraints derived from the CBF condition:

Standard CBF: ḣ(q) + α(h(q)) ≥ 0 Discrete time: J_h · δq/dt + α(h(q)) ≥ 0 Rearranging: -J_h · δq ≤ dt · α(h(q)) QP form: G · δq ≤ b where G = -J_h/dt, b = α(h(q))

Uses a saturating class-K function: α(h) = γ·h / (1 + |h|) This provides bounded recovery force, preventing over-reaction when far from the boundary while giving smooth, proportional behavior near constraints.

Safe displacement regularization adds a QP objective term: (safe_displacement_gain / (2·‖J_h‖²)) · ‖δq - δq_safe‖²

This encourages the robot to move toward a known safe configuration when near constraint boundaries. The weighting by 1/‖J_h‖² normalizes the contribution based on how sensitive the barrier is to joint motion.

The safety_margin parameter provides a conservative buffer for hard constraints. When safety_margin > 0, the CBF constraint is tightened by this amount, meaning the barrier will begin to resist motion earlier (when h = safety_margin rather than h = 0). This compensates for linearization errors in the discrete-time formulation.

Subclassed by roboplan::PositionBarrier

Public Functions

explicit Barrier(double gain, double dt, double safe_displacement_gain = 1.0, double safety_margin = 0.0)

Constructor with barrier parameters.

Parameters:

  • gain – Barrier gain (gamma), controls aggressiveness

  • dt – Timestep for discrete-time formulation (must match control loop period)

  • safe_displacement_gain – Gain for safe displacement regularization

  • safety_margin – Conservative margin for hard constraint guarantee (default 0.0)

void initializeStorage(int num_barriers, int num_vars)

Initialize pre-allocated storage.

Parameters:

  • num_barriers – Number of barrier constraints this barrier produces

  • num_vars – Number of optimization variables (model.nv)

virtual int getNumBarriers(const Scene &scene) const = 0

Get the number of barrier constraints this barrier produces.

Parameters:

scene – The scene containing robot state and model

Returns:

Number of barrier constraint rows

virtual tl::expected<void, std::string> computeBarrier(const Scene &scene) = 0

Compute the barrier function values h(q)

Note

Barrier values h(q) >= 0 indicate safety; h(q) < 0 indicates violation

Parameters:

scene – The scene containing robot state and model

Returns:

void on success, error message on failure

virtual tl::expected<void, std::string> computeJacobian(const Scene &scene) = 0

Compute the barrier Jacobian J_h = dh/dq.

Parameters:

scene – The scene containing robot state and model

Returns:

void on success, error message on failure

virtual Eigen::VectorXd computeSafeDisplacement(const Scene &scene) const

Compute safe displacement for regularization.

Subclasses can override to provide a non-zero safe displacement that the robot will be encouraged to move toward when near constraint boundaries.

Parameters:

scene – The scene containing robot state and model

Returns:

Safe displacement vector (num_variables), default is zero

tl::expected<void, std::string> computeQpInequalities(const Scene &scene, Eigen::Ref<Eigen::MatrixXd> G, Eigen::Ref<Eigen::VectorXd> b)

Compute QP inequality constraints for this barrier.

Computes: G_b * delta_q <= b_b Where: G_b = -J_h / dt b_b = gain * h(q) (linear class-K function)

Parameters:

  • scene – The scene containing robot state and model

  • G – Output constraint matrix (pre-sized view: num_barriers x num_variables)

  • b – Output constraint upper bound vector (pre-sized view: num_barriers)

Returns:

void on success, error message on failure

tl::expected<void, std::string> computeQpObjective(const Scene &scene, Eigen::Ref<Eigen::MatrixXd> H, Eigen::Ref<Eigen::VectorXd> c)

Compute QP objective contribution for safe displacement regularization.

Computes: (safe_displacement_gain / (2·‖J_h‖²)) · ‖δq - δq_safe‖²

This encourages the robot to move toward a safe configuration when near constraint boundaries. The weighting by 1/‖J_h‖² normalizes the contribution based on how sensitive the barrier is to joint motion.

Parameters:

  • scene – The scene containing robot state and model

  • H – Output Hessian matrix contribution (num_variables x num_variables)

  • c – Output gradient vector contribution (num_variables)

Returns:

void on success, error message on failure

virtual tl::expected<double, std::string> evaluateAtConfiguration(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q) const

Evaluate the minimum barrier value at a candidate configuration using FK.

This method allows post-solve validation by computing the actual barrier value at a candidate configuration q, independent of the linearized constraint used in the QP. Used by Oink::enforceBarriers() to detect linearization errors.

Parameters:

  • model – Pinocchio model

  • data – Pinocchio data (will be modified by FK computation)

  • q – Candidate joint configuration to evaluate

Returns:

Expected containing minimum barrier value across all barrier constraints, or infinity if this barrier type does not support configuration evaluation. Returns error message if evaluation fails (e.g., frame not found).

virtual ~Barrier() = default

Public Members

const double gain

Barrier gain (gamma)

const double dt

Timestep.

const double safe_displacement_gain

Gain for safe displacement regularization.

const double safety_margin

Conservative margin for hard constraints.

int num_variables = 0
Eigen::VectorXd barrier_values

Pre-allocated containers.

h(q) values (num_barriers)

Eigen::MatrixXd jacobian_container

J_h matrix (num_barriers x num_variables)