Struct Task

Inheritance Relationships

Derived Types

Struct Documentation

struct Task

Abstract base class for IK tasks.

Each task owns pre-allocated storage for Jacobian, error, and H_dense matrices. Subclasses must:

  1. Call initializeStorage() in their constructor with correct dimensions

  2. Implement computeJacobian() to fill jacobian_container

  3. Implement computeError() to fill error_container

Subclassed by roboplan::ConfigurationTask, roboplan::FrameTask

Public Functions

inline Task(Eigen::MatrixXd weight_matrix, double task_gain = 1.0, double lm_damp = 0.0)
virtual ~Task() = default
inline void initializeStorage(int task_rows, int num_vars)

Initialize pre-allocated storage with correct dimensions.

Parameters:

  • task_rows – Number of rows for the task (e.g., 6 for SE(3), nv for configuration)

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

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

Compute the task Jacobian and store in jacobian_container.

Parameters:

scene – The scene containing robot model and state.

Returns:

void on success, error message on failure.

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

Compute the task error and store in error_container.

Parameters:

scene – The scene containing robot model and state.

Returns:

void on success, error message on failure.

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

Compute QP objective matrices (H, c) for this task.

Computes the contribution of this task to the quadratic program objective: minimize ½ ‖J Δq + α e‖²_W

This is equivalent to: minimize ½ Δq^T H Δq + c^T Δq

Where:

  • J: Task Jacobian matrix

  • Δq: Configuration displacement

  • α: Task gain for low-pass filtering

  • e: Task error vector

  • W: Weight matrix for cost normalization

The method returns:

  • H = J_w^T J_w + μ I (num_variables x num_variables Hessian matrix, sparse)

  • c = -J_w^T e_w (num_variables x 1 linear term)

Where J_w = W*J, e_w = -α*W*e, and μ is the Levenberg-Marquardt damping.

Parameters:

  • scene – The scene containing robot model and state.

  • H – Output Hessian matrix (sparse)

  • c – Output linear cost term

Returns:

void on success, error message on failure.

Public Members

const double gain = 1.0
const Eigen::MatrixXd weight
const double lm_damping = 0.0
int num_variables = 0
Eigen::MatrixXd jacobian_container

Pre-allocated Jacobian container (task_rows × num_variables).

Eigen::VectorXd error_container

Pre-allocated error container (task_rows).

Eigen::MatrixXd H_dense

Pre-allocated dense Hessian matrix (num_variables × num_variables).