Struct FrameTask

Inheritance Relationships

Base Type

Struct Documentation

struct FrameTask : public roboplan::Task

Task for tracking a target Cartesian pose with a specified frame.

This task computes the SE(3) error between a target pose and the current frame pose, enabling full 6-DOF (position + orientation) tracking.

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

Public Functions

FrameTask(const Oink &oink, const Scene &scene, const CartesianConfiguration &target_pose, const FrameTaskOptions &options = {})

Constructs a FrameTask for tracking a target pose.

The Oink solver provides the velocity indices (for Jacobian column selection). The scene is used at construction time to resolve the frame ID and allocate the full Jacobian buffer.

Parameters:

  • oink – The Oink solver instance this task will be used with.

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

  • target_pose – The target Cartesian configuration to reach.

  • options – Optional task options (default: all options set to defaults).

Throws:

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

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

Computes the SE(3) error between target and current frame pose.

The error is computed as the logarithm of the relative transform: error = log_6(T_frame_to_world^{-1} * T_target_to_world)

Results are stored in error_container.

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 frame tracking task.

The task Jacobian J(q) ∈ ℝ^(6 × n_v) is the derivative of the task error e(q) ∈ ℝ^6 with respect to the configuration q. The formula is: 

J(q) = -Jlog_6(T_frame_to_target) * J_frame(q)
Where:

  • T_frame_to_target: Transform from current frame to target

  • J_frame(q): Frame Jacobian (expressed in frame coordinates)

  • Jlog_6: Pinocchio’s logarithmic Jacobian

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.

inline void setTargetFrameTransform(const Eigen::Matrix4d &tform)

Sets the target transform for this frame task.

Parameters:

tform – The target transform.

Public Members

std::string frame_name

Name of the frame to track (e.g., end-effector link name).

pinocchio::Index frame_id

Index of the frame in the scene’s Pinocchio model.

Eigen::VectorXi v_indices

Velocity vector indices for the joint group (used to select Jacobian columns).

CartesianConfiguration target_pose

Target Cartesian configuration to reach.

double max_position_error

Maximum position error magnitude (meters). Infinite means no limit.

double max_rotation_error

Maximum rotation error magnitude (radians). Infinite means no limit.

mutable Eigen::MatrixXd full_jacobian
mutable Eigen::Matrix<double, 6, 6> Jlog = Eigen::Matrix<double, 6, 6>::Identity()

Public Static Functions

static Eigen::MatrixXd createWeightMatrix(double position_cost, double orientation_cost)

Creates a diagonal weight matrix from scalar cost weights.

The weight matrix W ∈ ℝ^(6 × 6) is constructed as: W = diag(√position_cost * I_3, √orientation_cost * I_3)

Parameters:

  • position_cost – Cost weight for position error (first 3 dimensions).

  • orientation_cost – Cost weight for orientation error (last 3 dimensions).

Returns:

A 6×6 diagonal weight matrix.