Struct Oink
Defined in File optimal_ik.hpp
Struct Documentation
-
struct Oink
Oink - Optimal Inverse Kinematics solver.
Public Functions
-
Oink(const Scene &scene, const std::string &group_name)
Constructs an Oink solver for a named joint group.
Resolves the group to its velocity indices and sizes all internal matrices to the group’s velocity DOF count, which can be much smaller than model.nv when planning for a subset of joints.
- Parameters:
scene – The scene used to resolve the group at construction time.
group_name – Joint group name. Pass an empty string for the full robot.
- Throws:
std::runtime_error – if group_name is not found in the scene.
-
Oink(const Scene &scene, const std::string &group_name, const OsqpEigen::Settings &custom_settings)
Constructs an Oink solver for a named joint group with custom OSQP settings.
- Parameters:
scene – The scene used to resolve the group at construction time.
group_name – Joint group name. Pass an empty string for the full robot.
custom_settings – Custom OSQP solver settings.
- Throws:
std::runtime_error – if group_name is not found in the scene.
-
inline explicit Oink(const Scene &scene)
Constructs an Oink solver for the full robot (all joints).
Equivalent to Oink(scene, “”).
-
inline Oink(const Scene &scene, const OsqpEigen::Settings &custom_settings)
Constructs an Oink solver for the full robot with custom OSQP settings.
Equivalent to Oink(scene, “”, custom_settings).
Solve inverse kinematics for tasks only.
Solves a QP optimization problem to compute the joint velocity that minimizes weighted task errors.
- Parameters:
scene – The scene containing robot model and state
tasks – Vector of weighted tasks to optimize for
delta_q – Pre-allocated output buffer for configuration displacement
regularization – Tikhonov regularization weight (default: 1e-12)
- Returns:
void on success, error message on failure
Solve inverse kinematics for tasks with constraints.
Solves a QP optimization problem to compute the joint velocity that minimizes weighted task errors while satisfying all constraints.
- Parameters:
scene – The scene containing robot model and state
tasks – Vector of weighted tasks to optimize for
constraints – Vector of constraints to satisfy
delta_q – Pre-allocated output buffer for configuration displacement
regularization – Tikhonov regularization weight (default: 1e-12)
- Returns:
void on success, error message on failure
Solve inverse kinematics for tasks with barriers.
Solves a QP optimization problem to compute the joint velocity that minimizes weighted task errors while satisfying all barrier functions.
- Parameters:
scene – The scene containing robot model and state
tasks – Vector of weighted tasks to optimize for
barriers – Vector of barrier functions for safety constraints
delta_q – Pre-allocated output buffer for configuration displacement
regularization – Tikhonov regularization weight (default: 1e-12)
- Returns:
void on success, error message on failure
Solve inverse kinematics for tasks with constraints and barriers.
Solves a QP optimization problem to compute the joint velocity that minimizes weighted task errors while satisfying all constraints and barrier functions. The result is written directly into the provided delta_q buffer.
Note
The delta_q parameter must be pre-allocated to the correct size before calling. Eigen::Ref cannot be resized, so passing an empty or incorrectly sized vector will result in a failure.
- Parameters:
scene – The scene containing robot model and state
tasks – Vector of weighted tasks to optimize for
constraints – Vector of constraints to satisfy
barriers – Vector of barrier functions for safety constraints
delta_q – Pre-allocated output buffer for configuration displacement. Must be sized to num_variables (velocity space dimension). Using Eigen::Ref allows zero-copy access from Python numpy arrays.
regularization – Tikhonov regularization weight added to the Hessian diagonal. This provides numerical stability by ensuring the Hessian is strictly positive definite. Higher values increase regularization but may reduce task tracking accuracy. Default is 1e-12.
- Returns:
void on success, error message on failure
Validate delta_q against barriers using forward kinematics.
This method provides a post-solve safety check by evaluating the actual barrier values at the candidate configuration (q + delta_q). If any barrier would be violated, delta_q is set to zero to prevent unsafe motion.
This is a backup safety mechanism for cases where the linearized CBF constraint in the QP has significant error (e.g., large jumps, near-boundary configurations). The QP constraint uses a first-order approximation h(q + δq) ≈ h(q) + J_h · δq, which can have significant error O(||δq||²) for large displacements.
Note
Only barriers that implement evaluateAtConfiguration() are checked. Barriers returning infinity are assumed safe.
- Parameters:
scene – The scene containing robot model and state
barriers – Vector of barrier functions to check
delta_q – Configuration displacement to validate. Modified in place: set to zero if barrier violation is detected.
tolerance – Tolerance for barrier violation detection. A barrier is considered violated if h(q + delta_q) < -tolerance. Default is 0.0.
- Returns:
void on success, error message if barrier evaluation fails
Public Members
-
OsqpEigen::Solver solver
-
OsqpEigen::Settings settings
-
int num_variables
-
Eigen::VectorXi q_indices
Position indices of the joint group (used to scatter group q into model.nq space).
-
Eigen::VectorXi v_indices
Velocity indices of the joint group (used to scatter delta_q back into model.nv space).
-
Eigen::VectorXd task_c
-
Eigen::SparseMatrix<double> task_H
-
Eigen::SparseMatrix<double> H
-
Eigen::VectorXd c
-
Eigen::MatrixXd constraint_workspace_A
-
Eigen::VectorXd constraint_workspace_lower
-
Eigen::VectorXd constraint_workspace_upper
-
Eigen::SparseMatrix<double> A_sparse
-
std::vector<int> constraint_sizes
-
int last_constraint_rows = -1
-
Eigen::MatrixXd barrier_workspace_G
-
Eigen::VectorXd barrier_workspace_h
-
std::vector<int> barrier_sizes
-
int last_barrier_rows = 0
-
Eigen::MatrixXd barrier_H_contribution
-
Eigen::VectorXd barrier_c_contribution
-
Oink(const Scene &scene, const std::string &group_name)