Struct ConfigurationTask

Inheritance Relationships

Base Type

Struct Documentation

struct ConfigurationTask : public roboplan::Task

Task for tracking a target joint configuration.

This task computes the error between a target configuration and the current configuration in the tangent space, enabling joint-space tracking with per-joint weights.

The task owns pre-allocated storage for its nv×nv Jacobian and nv error vector, allocated at construction time to avoid runtime allocations during IK solving.

Public Functions

ConfigurationTask(const Oink &oink, const Eigen::VectorXd &target_q, const Eigen::VectorXd &joint_weights, const ConfigurationTaskOptions &options = {})

Constructs a ConfigurationTask for tracking a target configuration.

Pre-allocates storage for the (nv_group × nv_group) Jacobian and nv_group error vector. The group’s velocity indices are taken from the Oink solver to correctly extract sub-group errors from the full-robot tangent space.

Parameters:

  • oink – The Oink solver this task will be used with (provides q_indices, v_indices).

  • target_q – The target joint configuration for the group (size oink.q_indices.size()).

  • joint_weights – Per-joint weights for the group joints. Size must equal oink.num_variables. All weights must be non-negative.

  • options – Optional task options.

Throws:

std::invalid_argument – if joint_weights size doesn’t match oink.num_variables or if any joint weight is negative.

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

Computes the configuration space error. 

error = pin.difference(model, q_current, q_target)

Parameters:

scene – The scene containing the robot model and current state.

Returns:

Void if successful, else an error message string.

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

Computes the task Jacobian for the configuration task.

The task Jacobian J(q) ∈ ℝ^(nv × nv) is the negative identity matrix (-I). The negative sign ensures that the QP formulation (minimize ||J*dq + α*e||²) produces movement toward the target configuration.

Results are stored in jacobian_container.

Parameters:

scene – The scene containing the robot model and current state.

Returns:

Void if successful, else an error message string.

Public Members

Eigen::VectorXd target_q

Target joint configuration to reach.

Eigen::VectorXd joint_weights

Per-joint weights for cost function (one per group velocity DOF).

Eigen::VectorXi q_indices

Position indices of the joint group.

Eigen::VectorXi v_indices

Velocity indices of the joint group.

Public Static Functions

static Eigen::MatrixXd createWeightMatrix(const Eigen::VectorXd &joint_weights)

Creates a diagonal weight matrix from per-joint weights.

The weight matrix W ∈ ℝ^(nv × nv) is constructed as: W = diag(√joint_weights[i])

Parameters:

joint_weights – Per-joint weight vector.

Returns:

A nv×nv diagonal weight matrix.