IK_NR.error
- IK_NR.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.