ctrlutils.orientation_error¶

ctrlutils.orientation_error(quat: Eigen::Quaternion<double, 0>, quat_des: Eigen::Quaternion<double, 0>) → numpy.ndarray[numpy.float64[3, 1]]¶

Computes the quaternion error between the given orientations.

This quaternion error is equivalent to the angular velocity required to go from quat_des to quat in one second. Following this angular velocity also produces the SLERP interpolation between the two orientations.

For orientation control, a control law might look like the following:

`{.cc} Eigen::Matrix3d J_w = AngularJacobian(ab); Eigen::Vector3d w_err = opspace::OrientationError(Orientation(ab), quat_des); Eigen::Vector3d dw = -kp_ori * w_err - kv_ori * J_w * ab.dq; Eigen::VectorXd tau = opspace::InverseDynamics(ab, J_w, dw); `

Parameters

quat – Current orientation.
quat_des – Desired orientation.

Returns

Quaternion error.