Struct PositionBarrier

Inheritance Relationships

Base Type

Struct Documentation

struct PositionBarrier : public roboplan::Barrier

Position barrier constraint for end-effector box constraint.

Constrains a frame’s position to remain within an axis-aligned bounding box: p_min <= p(q) <= p_max

This creates up to 6 barrier constraints (2 per enabled axis).

The barrier functions are: h_lower_i = p_i(q) - p_min_i (for min bounds) h_upper_i = p_max_i - p_i(q) (for max bounds)

Uses a saturating class-K function α(h) = γ·h/(1+|h|) for smooth behavior.

Safe displacement regularization encourages moving toward the center of the safe region.

Public Functions

PositionBarrier(const Oink &oink, const Scene &scene, const std::string &frame_name, const Eigen::Vector3d &p_min, const Eigen::Vector3d &p_max, double dt, const ConstraintAxisSelection &axis_selection = ConstraintAxisSelection(), double gain = 1.0, double safe_displacement_gain = 1.0, double safety_margin = 0.0)

Constructs a position barrier for box constraint.

Note

The dt parameter significantly affects barrier behavior - ensure it matches your actual control/integration timestep.

Parameters:

  • oink – The Oink solver this barrier will be used with (provides num_variables and v_indices).

  • scene – The scene used to resolve the frame ID and allocate storage.

  • frame_name – Name of the frame to constrain.

  • p_min – Minimum position bounds [x, y, z] in world frame (use -inf for no constraint).

  • p_max – Maximum position bounds [x, y, z] in world frame (use +inf for no constraint).

  • dt – Timestep matching your control loop period (required; must match actual control loop).

  • axis_selection – Which axes to constrain (default: all three axes).

  • gain – Barrier gain (gamma), controls convergence to safe set. Default 1.0

  • safe_displacement_gain – Gain for safe displacement regularization. Default 1.0

  • safety_margin – Conservative margin for hard constraint guarantee. Default 0.0

Throws:

std::runtime_error – if frame_name is not found in the scene.

virtual int getNumBarriers(const Scene &scene) const override

Get the number of active barrier constraints.

Parameters:

scene – The scene containing robot model and state.

Returns:

Number of active barriers (up to 6: 2 per enabled axis).

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

Compute barrier function values h(q) for all active constraints.

Evaluates the barrier functions:

  • h_lower_i = p_i(q) - p_min_i (for min bounds on enabled axes)

  • h_upper_i = p_max_i - p_i(q) (for max bounds on enabled axes)

Also computes right-hand side bounds using the saturating class-K function: rhs_i = dt * gamma * h_i / (1 + |h_i|) - safety_margin

Results are stored in the inherited barrier_values and barrier_rhs vectors.

Parameters:

scene – The scene containing robot model and current state.

Returns:

Expected void on success, or error message if frame is not found.

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

Compute barrier constraint Jacobian matrix.

Computes the Jacobian -J_h used in the QP constraint: -J_h * delta_q <= rhs Each barrier uses one row of the frame’s position Jacobian (first 3 rows only). The sign is negated for upper bounds to convert p_max - p(q) >= 0 into the standard form.

Results are stored in the inherited barrier_jacobian matrix (num_barriers x nv).

Parameters:

scene – The scene containing robot model and current state.

Returns:

Expected void on success, or error message if frame is not found.

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

Evaluate minimum barrier value at a candidate configuration.

Computes forward kinematics for the candidate configuration and returns the minimum barrier value across all position constraints (x, y, z min/max).

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 (negative if any constraint is violated), or error message if frame is not found

Eigen::Vector3d getFramePosition(const Scene &scene) const

Get current frame position in world coordinates.

Parameters:

scene – The scene containing robot state.

Returns:

Frame position in world coordinates.

Public Members

const std::string frame_name

Name of the frame to constrain.

const ConstraintAxisSelection axis_selection

Axis selection for constraints (x, y, z).

const Eigen::Vector3d p_min

Minimum position bounds in world frame for each axis.

const Eigen::Vector3d p_max

Maximum position bounds in world frame for each axis.

Eigen::VectorXi v_indices

Velocity indices of the joint group (for Jacobian column selection).

pinocchio::FrameIndex frame_id

Frame index (resolved eagerly at construction).

mutable Eigen::MatrixXd full_jacobian

Pre-allocated full-robot Jacobian workspace (6 x model.nv).