roll Archives - Robotics with ROS

Finding roll, pitch yaw from 3X3 rotation matrix with Eigen

/*  http://planning.cs.uiuc.edu/node103.html

    |r11 r12 r13 |
    |r21 r22 r23 |
    |r31 r32 r33 |

    yaw: alpha=arctan(r21/r11)
    pitch: beta=arctan(-r31/sqrt( r32^2+r33^2 ) )
    roll: gamma=arctan(r32/r33)
*/
    double roll, pitch, yaw;
    roll=M_PI/3;
    pitch=M_PI/4;
    yaw=M_PI/6;
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaternion<double> q =  yawAngle*pitchAngle *rollAngle;
    Eigen::Matrix3d rotationMatrix = q.matrix();



    std::cout<<"roll is Pi/" <<M_PI/atan2( rotationMatrix(2,1),rotationMatrix(2,2) ) <<std::endl;
    std::cout<<"pitch: Pi/" <<M_PI/atan2( -rotationMatrix(2,0), std::pow( rotationMatrix(2,1)*rotationMatrix(2,1) +rotationMatrix(2,2)*rotationMatrix(2,2) ,0.5  )  ) <<std::endl;
    std::cout<<"yaw is Pi/" <<M_PI/atan2( rotationMatrix(1,0),rotationMatrix(0,0) ) <<std::endl;

/* http://planning.cs.uiuc.edu/node103.html

|r11 r12 r13 |

|r21 r22 r23 |

|r31 r32 r33 |

yaw: alpha=arctan(r21/r11)

pitch: beta=arctan(-r31/sqrt( r32^2+r33^2 ) )

roll: gamma=arctan(r32/r33)

double roll, pitch, yaw;

roll=M_PI/3;

pitch=M_PI/4;

yaw=M_PI/6;

Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());

Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());

Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

Eigen::Quaternion<double> q = yawAngle*pitchAngle *rollAngle;

Eigen::Matrix3d rotationMatrix = q.matrix();

std::cout<<"roll is Pi/" <<M_PI/atan2( rotationMatrix(2,1),rotationMatrix(2,2) ) <<std::endl;

std::cout<<"pitch: Pi/" <<M_PI/atan2( -rotationMatrix(2,0), std::pow( rotationMatrix(2,1)*rotationMatrix(2,1) +rotationMatrix(2,2)*rotationMatrix(2,2) ,0.5 ) ) <<std::endl;

std::cout<<"yaw is Pi/" <<M_PI/atan2( rotationMatrix(1,0),rotationMatrix(0,0) ) <<std::endl;

Roll, pitch, yaw using Eigen and KDL Frame

Leave a Comment / C++, Linear Algebra, ROS, Tutorials / admin

/*
yaw:
    A yaw is a counterclockwise rotation of alpha about the  z-axis. The rotation matrix is given by

    R_z

    |cos(alpha) -sin(alpha) 0|
    |sin(apha)   cos(alpha) 0|
    |    0            0     1|

pitch:
    R_y
    A pitch is a counterclockwise rotation of  beta about the  y-axis. The rotation matrix is given by

    |cos(beta)  0   sin(beta)|
    |0          1       0    |
    |-sin(beta) 0   cos(beta)|

roll:
    A roll is a counterclockwise rotation of  gamma about the  x-axis. The rotation matrix is given by
    R_x
    |1          0           0|
    |0 cos(gamma) -sin(gamma)|
    |0 sin(gamma)  cos(gamma)|



    It is important to note that   R_z R_y R_x performs the roll first, then the pitch, and finally the yaw
    Roration matrix: R_z*R_y*R_x
*/

    double roll, pitch, yaw;
    roll=M_PI/3;
    pitch=M_PI/4;
    yaw=M_PI/6;
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());




    Eigen::Quaternion<double> q =  yawAngle*pitchAngle *rollAngle;

    Eigen::Matrix3d rotationMatrix = q.matrix();
    std::cout<<rotationMatrix <<std::endl;

yaw:

A yaw is a counterclockwise rotation of alpha about the z-axis. The rotation matrix is given by

R_z

|cos(alpha) -sin(alpha) 0|

|sin(apha) cos(alpha) 0|

| 0 0 1|

pitch:

R_y

A pitch is a counterclockwise rotation of beta about the y-axis. The rotation matrix is given by

|cos(beta) 0 sin(beta)|

|0 1 0 |

|-sin(beta) 0 cos(beta)|

roll:

A roll is a counterclockwise rotation of gamma about the x-axis. The rotation matrix is given by

R_x

|1 0 0|

|0 cos(gamma) -sin(gamma)|

|0 sin(gamma) cos(gamma)|

It is important to note that R_z R_y R_x performs the roll first, then the pitch, and finally the yaw

Roration matrix: R_z*R_y*R_x

double roll, pitch, yaw;

roll=M_PI/3;

pitch=M_PI/4;

yaw=M_PI/6;

Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());

Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());

Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

Eigen::Quaternion<double> q = yawAngle*pitchAngle *rollAngle;

Eigen::Matrix3d rotationMatrix = q.matrix();

std::cout<<rotationMatrix <<std::endl;

From Eigen documentation: If you are working with OpenGL 4×4 matrices then Affine3f and Affine3d are what you want. Since Eigen defaults to column-major storage, you can directly use the Transform::data() method to pass your transformation matrix to OpenGL. construct a Transform:

Transform t(AngleAxis(angle,axis));

1	Transform t(AngleAxis(angle,axis));

or like this:

Transform t;
t = AngleAxis(angle,axis);

1 2	Transform t; t = AngleAxis(angle,axis);

But note that unfortunately, because of

Roll, pitch, yaw using Eigen and KDL Frame Read More »

Cookie	Duration	Description
cookielawinfo-checbox-analytics	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Analytics".
cookielawinfo-checbox-functional	11 months	The cookie is set by GDPR cookie consent to record the user consent for the cookies in the category "Functional".
cookielawinfo-checbox-others	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Other.
cookielawinfo-checkbox-necessary	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookies is used to store the user consent for the cookies in the category "Necessary".
cookielawinfo-checkbox-performance	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Performance".
viewed_cookie_policy	11 months	The cookie is set by the GDPR Cookie Consent plugin and is used to store whether or not user has consented to the use of cookies. It does not store any personal data.