Rotate a quaternion by Euler angles input

Question

I am writing a code to control robotic arm in 3D space. The robotic arm handle the rotation by quaternion but I want user to control it by changing yaw, pitch and roll since its more sensible for human to use these.

I wrote function to get the amount that user wants to rotate the arm in each of directions(roll, pitch, yaw) and output the new quaternion. I saved the current_quaternion as a global variable.

I am using C++ and Eigen.

Eigen::Quaterniond euler2Quaternion( const double roll,
              const double pitch,
              const double yaw)
{

Eigen::AngleAxisd rollAngle(roll,Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(pitch,Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(yaw,Eigen::Vector3d::UnitZ());

Eigen::Quaterniond q = rollAngle*pitchAngle*yawAngle;
current_q=q*current_q;
return current_q;
}

I tried many things changing the order of multiplying angles and multiplying UnitX(), UnitY() and UnitZ() by current_q.toRotationMatrix() but non of them worked.

What do you mean "none of them worked"? As in, it didn't produce the expected transform? — Drew McGowen
yeah, the rotations are always changing even when all the inputs are zero. — D3GAN
What are the AngleAxisd? the Quaterniond? Do they make sense? — Avi Ginsburg
What about simply storing the euler angles (roll, pitch, yaw) instead of current_q, and generate the quaternion from the euler angles when needed? — sbabbi
@Avi Ginsburg : I got the base for my code from this question: stackoverflow.com/questions/21412169/… — D3GAN

Johan Lundberg Johan Lundberg · Accepted Answer · 2015-04-23T17:18:09

Your example is almost identical to the example

Matrix3f m;
m = AngleAxisf(0.25*M_PI, Vector3f::UnitX())
  * AngleAxisf(0.5*M_PI,  Vector3f::UnitY())
  * AngleAxisf(0.33*M_PI, Vector3f::UnitZ());

Have you tried printing the result of that combined rotation matrix? I will bet it is diagonal 1,1,1 when the angles are zero.

I'm confused about your use of current_q. If roll, pitch, yaw corresponds to some fixed reference direction, then q will be the whole rotation. In that case, this:

current_q=q*current_q;
return current_q;

should just be

current_q=q;
return current_q;

if roll, pitch, yaw are meant as changes to the current euler rotation angles (which start from some fixed reference direction), it's easier to store these angles and change them accordingly:

double m_roll=0, m_pitch=0, m_yaw=0;
 . . .
Eigen::Quaterniond euler2Quaternion(double roll,
              double pitch,
              double yaw)
{
 m_roll+=roll;
 m_pitch+=pitch;
 m_yaw+=yaw;

 Eigen::AngleAxisd rollAngle(m_roll,Eigen::Vector3d::UnitX());
 Eigen::AngleAxisd pitchAngle(m_pitch,Eigen::Vector3d::UnitY());
 Eigen::AngleAxisd yawAngle(m_yaw,Eigen::Vector3d::UnitZ());

 Eigen::Quaterniond q = rollAngle*pitchAngle*yawAngle;
 current_q=q;
 return current_q;
}

This is also suggested in a comment by sbabbi

Rotate a quaternion by Euler angles input

2 Answers