[WBC 03] - From SE(3) Error to Task-Space PD Control

How a pose target becomes a 6D task-space error, desired acceleration, and equality constraint in a WBC solver.

WBC03. From SE(3) Error to Task-Space PD Control

In the previous post, we looked at how to represent floating-base robots in code.

The main point was that configuration, velocity, acceleration, actuator torque, and contact force do not all live in the same space. For a floating-base robot, q, v, qddot, and tau have different meanings and often different dimensions.

Now we move one layer up.

Once the robot state is represented correctly, we need to define tasks.

A common task is simple to describe in words:

Move this frame to that desired pose.

For example:

Move the right hand frame to a target SE(3) pose.

But this is not what the WBC solver directly receives.

A WBC solver does not usually solve:

frame_pose = desired_pose

Instead, an acceleration-level WBC task converts pose and velocity errors into a desired task acceleration, then exposes that desired acceleration as a linear constraint on generalized acceleration.

The final constraint usually looks like this:

J_task(q) qddot = a_des - drift

This post explains how we get there.

The core idea is:

A pose task does not send a desired pose to the solver. It converts an SE(3) pose error into a tangent-space acceleration target.

---

1. A task is not just x_des

It is tempting to think of a task as a target.

task.setTargetPose(x_des);

That is only the user-facing part.

Internally, a WBC task must provide something the solver can use. For an acceleration-level inverse-dynamics solver, this usually means a constraint of the form:

\[J(q)\ddot{q} = a_{des} - a_{drift}\]

In code, this becomes something like:

constraint.matrix() = J_task;
constraint.vector() = a_des - drift;

The target pose itself is not enough.

The task needs to compute:

current pose
desired pose
pose error
current frame velocity
desired frame velocity
velocity error
desired task acceleration
Jacobian
drift acceleration
masked constraint rows

So a task is better understood as a small controller.

It reads the robot state, computes task-space error, applies feedback gains, and produces a solver constraint.

A rough flow is:

desired pose
+ current pose
→ pose error

desired velocity
+ current velocity
→ velocity error

pose error
+ velocity error
+ Kp/Kd
→ desired task acceleration

desired task acceleration
+ Jacobian
+ drift
→ solver equality constraint

In LaTeX:

\[e = x_{des} \ominus x\] \[e_v = v_{des} - v\] \[a_{des} = K_p e + K_d e_v + a_{ref}\] \[J(q)\ddot{q} = a_{des} - a_{drift}\]

In code, the same idea looks more like:

poseError(current_pose, desired_pose, pose_error);

velocity_error = desired_velocity_local - current_velocity_local;

a_des =
  Kp.cwiseProduct(pose_error.toVector()) +
  Kd.cwiseProduct(velocity_error.toVector()) +
  desired_acceleration_local.toVector();

constraint.matrix().row(idx) = J.row(i);
constraint.vector().row(idx) = (a_des - drift).row(i);

This is the first important software boundary:

The task owns error computation and desired acceleration. The solver only sees a linearized acceleration constraint.

---

2. SE(3) pose is not a 6D vector

A 3D pose is usually represented as an element of SE(3).

In math:

\[T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \in SE(3)\]

where:

\[R \in SO(3), \qquad p \in \mathbb{R}^3\]

In code, Pinocchio represents this using pinocchio::SE3.

pinocchio::SE3 T;

Eigen::Matrix3d R = T.rotation();
Eigen::Vector3d p = T.translation();

This is the actual pose.

It is not the same as a task error.

That distinction matters.

A pose is a transformation. An error is a tangent-space vector used by a controller.

In other words:

SE(3):
  actual pose / transformation

se(3):
  local 6D motion / error / velocity-like quantity

The task compares two SE(3) poses:

pinocchio::SE3 current;
pinocchio::SE3 desired;

But the controller eventually wants a 6D vector:

pinocchio::Motion error;
Eigen::Matrix<double, 6, 1> error_vec = error.toVector();

This 6D vector is what can be multiplied by Kp.

That is why the pose must be converted into an error first.

---

3. Relative transform: comparing current and desired pose

Suppose the current frame pose is:

\[T_{current}\]

and the desired frame pose is:

\[T_{desired}\]

The task first computes the relative transform:

\[T_{err} = T_{current}^{-1} T_{desired}\]

In code:

const pinocchio::SE3 transform_error =
  current.actInv(desired);

This is equivalent to:

transform_error = current.inverse() * desired;

Conceptually, T_err answers:

From the current frame, where is the desired frame?

If:

\[T_{err} = \begin{bmatrix} R_{err} & p_{err} \\ 0 & 1 \end{bmatrix}\]

then the task can extract translation and rotation error from this relative transform.

The current implementation uses a split error:

\[e = \begin{bmatrix} p_{err} \\ \log(R_{err})^\vee \end{bmatrix}\]

In code:

error.linear() = transform_error.translation();
error.angular() = pinocchio::log3(transform_error.rotation());

error_vec = error.toVector();

The side-by-side mapping is:

Math	Code
$T_{err} = T_{current}^{-1}T_{desired}$	current.actInv(desired)
$p_{err}$	transform_error.translation()
$R_{err}$	transform_error.rotation()
$\log(R_{err})^\vee$	pinocchio::log3(transform_error.rotation())
$e = [p_{err}; \log(R_{err})^\vee]$	error.toVector()

This error is now a 6D vector that the controller can use.

---

4. Why rotation needs log3

Translation error is easy to understand.

If the current position is:

\[p = \begin{bmatrix}1 \\ 0 \\ 0\end{bmatrix}\]

and the desired position is:

\[p_{des} = \begin{bmatrix}2 \\ 0 \\ 0\end{bmatrix}\]

then the position error is roughly:

\[e_p = \begin{bmatrix}1 \\ 0 \\ 0\end{bmatrix}\]

Rotation is different.

A rotation matrix is not a normal vector. It is an element of SO(3):

\[SO(3) = \left{ R \in \mathbb{R}^{3 \times 3} \mid R^\top R = I,\ \det(R)=1 \right}\]

So this is not a good orientation error:

// Avoid
Eigen::Matrix3d R_error_bad = R_desired - R_current;

The result is just a matrix difference. It does not directly answer:

Around which axis should I rotate, and by how much?

For control, we want a 3D angular error vector.

That vector lives in the Lie algebra so(3).

The logarithm map converts a relative rotation into this 3D angular error:

\[e_R = \log(R_{err})^\vee\]

In code:

Eigen::Vector3d e_R =
  pinocchio::log3(R_error);

If the desired frame is rotated by a small angle around the z-axis, this error may look like:

[0, 0, 0.1]

That is exactly the kind of vector a PD controller can use.

So the pipeline is:

SO(3) rotation error
→ log3
→ 3D angular error
→ Kp/Kd control

This is why orientation error is not computed by subtracting quaternion coefficients or rotation matrices.

---

5. Split error versus log6

There are two common ways to create an SE(3) pose error.

The current implementation uses a split error:

\[e_{split} = \begin{bmatrix} p_{err} \\ \log(R_{err})^\vee \end{bmatrix}\]

Code:

const pinocchio::SE3 transform_error =
  current.actInv(desired);

error.linear() = transform_error.translation();
error.angular() = pinocchio::log3(transform_error.rotation());

Another option is to use the full SE(3) logarithm:

\[e_{log6} = \log(T_{err})^\vee = \begin{bmatrix} \rho \\ \phi \end{bmatrix}\]

Code:

pinocchio::Motion error =
  pinocchio::log6(transform_error);

Both produce a 6D vector.

But they are not the same.

The angular part is closely related:

\[\phi \approx \log(R_{err})^\vee\]

But the linear part is different.

For the split error:

\[e_{split, linear} = p_{err}\]

For the full logarithm:

\[e_{log6, linear} = \rho\]

The value $\rho$ is not always equal to $p_{err}$. It is the linear component of the SE(3) logarithm, coupled with the rotational part of the transform.

So changing from split error to log6 is not a harmless refactor.

It changes the meaning of the task error.

Method	Math	Code	Linear part
Split error	$[p_{err};\ \log(R_{err})^\vee]$	translation() + log3(rotation())	relative translation
Full log6	$\log(T_{err})^\vee$	pinocchio::log6(T_err)	SE(3) logarithm linear component

The split version has a practical advantage: position and orientation gains remain easy to interpret.

linear gains:
  control relative translation

angular gains:
  control relative rotation

The log6 version treats the whole pose error as one screw-like 6D motion. That can be mathematically cleaner for some formulations, but the linear part is less directly interpreted as “move x/y/z by this amount.”

For a software stack, the important rule is:

Pick one error definition intentionally. Do not switch between split error and log6 without retuning gains and tests.

---

6. SE(3) storage vector is not task error

Another common source of confusion is that multiple things appear as Eigen::Vector.

For example, a project may store an SE(3) reference pose as a 12D vector:

\[x_{storage} = \begin{bmatrix} p \\ \operatorname{vec}(R) \end{bmatrix} \in \mathbb{R}^{12}\]

This can be useful for storage, logging, messages, or trajectory interfaces.

In code:

se3ToVector(m_M_ref, m_p_ref);
vectorToSE3(ref.pos, m_M_ref);

But this 12D vector is not a task error.

It is only a storage representation.

The actual task error is 6D:

\[e = \begin{bmatrix} e_{linear} \\ e_{angular} \end{bmatrix} \in \mathbb{R}^6\]

In code:

pinocchio::Motion error;

error.linear() = transform_error.translation();
error.angular() = pinocchio::log3(transform_error.rotation());

Eigen::Matrix<double, 6, 1> e = error.toVector();

A useful distinction is:

Quantity	Dimension	Meaning	Code
pinocchio::SE3	object	actual pose transform	oMi, m_M_ref
SE(3) storage vector	12	storage/display [p; vec(R)]	se3ToVector, vectorToSE3
task error vector	6	tangent-space error	pinocchio::Motion::toVector()
solver RHS	task dimension	masked equality target	constraint.vector()

So even if both are represented as vectors in C++, they are not the same type of object.

Flattened pose storage should not be subtracted to make a controller error.

---

7. From pose error to desired acceleration

Once we have a 6D pose error, the task can apply PD control.

Let:

\[e_{pose} = \begin{bmatrix} e_p \\ e_R \end{bmatrix}\]

and let the velocity error be:

\[e_v = v_{ref} - v_{current}\]

Then the desired task acceleration is:

\[\begin{aligned} a_{des} &= K_p \odot e_{pose} + K_d \odot e_v + a_{ref} \end{aligned}\]

Here, $\odot$ means element-wise multiplication.

In code:

m_a_des =
  m_Kp.cwiseProduct(m_p_error_vec) +
  m_Kd.cwiseProduct(m_v_error.toVector()) +
  m_a_ref_local.toVector();

This is not just a random formula.

It is the acceleration-level version of task-space PD control.

The position/orientation error creates a restoring acceleration. The velocity error creates damping. The reference acceleration adds feedforward motion.

The 6D ordering is usually:

[linear_x, linear_y, linear_z, angular_x, angular_y, angular_z]

So the gains can be interpreted axis by axis:

Kp[0:3]  -> translational stiffness-like gains
Kp[3:6]  -> rotational stiffness-like gains
Kd[0:3]  -> translational damping-like gains
Kd[3:6]  -> rotational damping-like gains

This is one reason model-based task-space control remains useful.

The gains have physical meaning.

They are not perfect physical impedance unless the full dynamics shaping is implemented carefully, but they are still interpretable knobs.

Increasing translational Kp makes the frame try harder to reduce position error. Increasing angular Kd damps rotational motion. Changing only the angular gains affects orientation behavior without changing translation behavior.

That interpretability is one of the main benefits of writing the task explicitly.

---

8. Frame convention matters

The error, velocity, Jacobian, and desired acceleration must be expressed in compatible frames.

A common choice is local frame control.

In local frame mode, the task uses the frame-local Jacobian and expresses errors in the current frame convention.

Code may look like:

m_robot.frameJacobianLocal(data, m_frame_id, m_J);

poseError(oMi, m_M_ref, m_p_error);

m_p_error_vec = m_p_error.toVector();

m_v_error = m_wMl.actInv(m_v_ref) - v_frame;

The desired acceleration is then computed in the same local convention:

m_a_des =
  m_Kp.cwiseProduct(m_p_error_vec) +
  m_Kd.cwiseProduct(m_v_error.toVector()) +
  m_wMl.actInv(m_a_ref).toVector();

The important point is not the exact function names.

The important point is consistency.

If the Jacobian maps qddot to local-frame acceleration, then a_des should also be in that local-frame convention.

Otherwise, the solver will satisfy the wrong equation.

Some implementations also support a local world-oriented frame. In that case, the origin follows the task frame, but the axes are aligned with the world frame.

Code may rotate the local quantities into a world-oriented representation:

m_p_error_vec = m_wMl.toActionMatrix() * m_p_error.toVector();
m_v_error = m_v_ref - m_wMl.act(v_frame);
m_drift = m_wMl.act(m_drift);
m_J = m_wMl.toActionMatrix() * m_J;

This changes how the task rows should be interpreted.

The software rule is:

Error, velocity, drift, and Jacobian rows must use the same frame convention.

If they do not, the gains may appear unstable or directionally wrong even when the math looks correct.

---

9. From desired acceleration to solver constraint

The task-space acceleration relationship is:

\[a_{task} = J(q)\ddot{q} + a_{drift}\]

The drift term is the part of frame acceleration caused by the current robot velocity. It plays a role similar to:

\[\dot{J}(q, v)v\]

The task wants:

\[a_{task} = a_{des}\]

Substitute the acceleration relationship:

\[J(q)\ddot{q} + a_{drift} = a_{des}\]

Move the drift term to the right-hand side:

\[J(q)\ddot{q} = a_{des} - a_{drift}\]

This is the equality constraint that enters the HQP or QP solver.

In code:

constraint.matrix().row(idx) =
  J.row(i);

constraint.vector().row(idx) =
  (a_des - drift).row(i);

With a mask, only selected rows are added.

For example:

[1, 1, 1, 0, 0, 0]  -> position-only task
[0, 0, 0, 1, 1, 1]  -> orientation-only task
[1, 1, 1, 1, 1, 1]  -> full SE(3) task

So the final task constraint is not necessarily 6D. It has as many rows as the active mask entries.

The solver then finds qddot such that the active task rows are satisfied as well as possible, depending on the hierarchy and other constraints.

A useful code-level check is the task acceleration reconstruction:

a_task = constraint.matrix() * qddot_sol + drift_masked;

If the task is satisfied, this should match:

a_des_masked

This is a good debugging tool.

When a task behaves incorrectly, check these in order:

Is the pose error in the expected frame?
Is the velocity error in the same frame?
Is the Jacobian expressed in the same frame?
Is drift being subtracted on the RHS?
Is the mask selecting the intended rows?
Is the gain applied to the correct row ordering?

Most bugs in SE(3) tasks are not solver bugs.

They are convention bugs.

---

10. LaTeX-to-code summary

The whole pipeline can be summarized as follows.

Step	LaTeX	Code
Current pose	$T_{current}$	oMi
Desired pose	$T_{desired}$	m_M_ref
Relative transform	$T_{err}=T_{current}^{-1}T_{desired}$	current.actInv(desired)
Translation error	$e_p=p_{err}$	transform_error.translation()
Rotation error	$e_R=\log(R_{err})^\vee$	pinocchio::log3(transform_error.rotation())
Pose error	$e=[e_p; e_R]$	error.toVector()
Velocity error	$e_v=v_{ref}-v$	m_v_error
Desired acceleration	$a_{des}=K_p\odot e+K_d\odot e_v+a_{ref}$	m_a_des = Kp.cwiseProduct(...) + Kd.cwiseProduct(...) + a_ref
Task acceleration	$a_{task}=J(q)\ddot{q}+a_{drift}$	J * qddot + drift
Solver equality	$J(q)\ddot{q}=a_{des}-a_{drift}$	constraint.matrix() = J, constraint.vector() = a_des - drift

The key transformation is:

SE(3) pose target
→ relative transform
→ 6D tangent-space error
→ desired task acceleration
→ equality constraint on qddot

That is what an SE(3) WBC task does.

---

11. Summary

An SE(3) task in WBC is not just a desired pose.

It is a small acceleration-level controller.

It compares the current frame pose and desired frame pose, builds a tangent-space pose error, applies task-space PD gains, and produces a linear equality constraint for the solver.

The important distinction is:

SE(3):
  pose / transform

se(3):
  local motion / error / velocity-like vector

The current split-error implementation uses:

\[e = \begin{bmatrix} p_{err} \\ \log(R_{err})^\vee \end{bmatrix}\]

rather than:

\[e = \log(T_{err})^\vee\]

This is an intentional control-law choice. It keeps translation and rotation error meanings separate, which makes masks and gain tuning easier to interpret.

After the pose error is built, task-space PD control creates:

\[\begin{aligned} a_{des} &= K_p \odot e_{pose} + K_d \odot e_v + a_{ref} \end{aligned}\]

Finally, the task enters the WBC solver as:

\[J(q)\ddot{q} = a_{des} - a_{drift}\]

This is the key idea.

The solver does not receive a pose.

It receives an acceleration-level constraint.

In the next post, we will look more carefully at what a Task object should own: target references, masks, gains, frame convention, Jacobian updates, drift terms, and constraint construction.