IK_QP.error

IK_QP.error(Te, Tep)

Calculates the error between Te and Tep

Calculates the engle axis error between current end-effector pose Te and the desired end-effector pose Tep. Also calulates the quadratic error E which is weighted by the diagonal matrix We.

\[E = \frac{1}{2} \vec{e}^{\top} \mat{W}_e \vec{e}\]

where \(\vec{e} \in \mathbb{R}^6\) is the angle-axis error.

Parameters:

• Te (ndarray) – The current end-effector pose

• Tep (ndarray) – The desired end-effector pose

Return type:

Tuple[ndarray, float]

Returns:

• e – angle-axis error (6 vector)

• E – The quadratic error weighted by We