2
$\begingroup$

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!

$\endgroup$

1 Answer 1

2
$\begingroup$

In general, Euler angles are one of the worst representations for orientation when you need to do math. Quaternions and axis-angle representation are closely related I believe. This is a good site for understanding quaternions: https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/index.htm And it also has pages on conversions: https://www.euclideanspace.com/maths/geometry/rotations/conversions/index.htm

But the answers in this thread might be of use to you too: Jacobian-based trajectory following

And if you want a really deep dive into quaternions, this is a good reference: http://www.neil.dantam.name/note/dantam-quaternion.pdf

$\endgroup$
1
  • 2
    $\begingroup$ +1 for Euler angles just being the worst. They're only intuitive if you're basically rotating just one axis, any more than that and they're basically as obtuse as quaternions except they're also wildly inconsistent. "Is that ypr or rpy rotation?" kill me lol Also those occasions where they use terms like rpy or ypr when really they're actually XYX Euler angles and not Tait Bryan angles. $\endgroup$
    – Chuck
    Oct 20, 2022 at 2:50

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.