I'm currently working on creating a ROS controller package for franka arm, which I choose to implement a feedback linearization controller that works as following:
Assume we have a planned trajectory $x_d$, planned velocity $\dot x_d$, and planned acceleration $\ddot x_d$. All of which are 6x1 matrices with upper three rows corresponding to position, and bottom three rows corresponding to orientation of the end effector.
In order to find the desired joint torque $\tau$, three most important quantities that we need to solve for are desired joint acceleration $\ddot q_d$, joint position and velocity errors $\Delta q$ and $\Delta\dot q$ respectively.
Whereas we can solve for them by the following steps: $$\begin{align} \Delta q = J^+(q)(x_d-x) \end{align}$$ $$\begin{align} \Delta\dot q = J^+(q)(\dot x_d-\dot x) \end{align}$$ $$\begin{align} \ddot q_d = J^+(q)(\ddot x_d - \dot J(q)\dot q) \end{align}$$ whereas for franka arm, the Jacobian matrix $J$ and its time derivative $\dot J$ are 6x7 matrices, and Jacobian psedoinverse $J^+$ is a 7x6 matrix.
To calculate joint torque, we have: $$\begin{align} \tau = M(\ddot q_d + K_p\Delta q+K_d\Delta \dot q) + \tau_G \end{align}$$
In terms of implementation, I had trouble calculating the bottow three rows of $x_d,\dot x_d,$ and $\ddot x_d$, which are the Euler position,velocity, and acceleration of the orientation of end effector respectively. Since the hardware interface of the robot only has an attribute to end effector orientation in quaternion, and the trajectory is in terms of Euler angle, I found this approach online:
if (Quaternion.coeffs().dot(orientation_curr_.coeffs()) < 0.0) {
orientation_curr_.coeffs() << -orientation_curr_.coeffs();
}
Eigen::Quaterniond error_quaternion(orientation_curr_.inverse() * Quaternion);
error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
error.tail(3) << curr_transform.linear() * error.tail(3);
whereas Quaternion is the desired orientation transforming from Euler angle by:
Quaternion = Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())* Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())* Eigen::AngleAxisd(ya, Eigen::Vector3d::UnitZ());
My question is can somebody please explain to me the mathematical intuition behind the code finding the error? Thank you so much in advance!
