I need to find the angle between two sets of Roll and Yaw angles

Question

I have a sensor attached to a drill. The sensor outputs orientation in heading roll and pitch. From what I can tell these are intrinsic rotations in that order. The Y axis of the sensor is parallel to the longitudinal axis of the drill bit. I want to take a set of outputs from the sensor and find the maximum change in orientation from the final orientation.

Since the drill bit will be spinning about the pitch axis, I believe it can be neglected.

My first thought would be to try to convert heading and roll to unit vectors assuming pitch is 0. Once I have their vectors, call them v and vf, the angle between them would be

Θ=arccos(v . vf)

It should then be fairly straight forward to have python calculate Θ for a given set of orientations and pull the largest out.

My question is, is there a simpler way to do this using python, and, if not what is the most efficient way to convert these intrinsic rotations to unit vectors.

Answer 1

I think that spherical trigonometry will help you. https://en.wikipedia.org/wiki/Spherical_trigonometry https://en.wikipedia.org/wiki/Haversine_formula#The_law_of_haversines

If I understand you correctly, the functions below should do the trick:

double AngleBetweenRollPitch(double roll1, double pitch1, double roll2, double pitch2) {
    return SphericalSideFromSidesAndIncludedAngle(pitch1, pitch2, roll2 - roll1);
}
double SphericalSideFromSidesAndIncludedAngle(double b, double c, double alpha) { /* the Law of haversines */
    return archaversin(haversin(b - c) + sin(b) * sin(c) * haversin(alpha));
}
double SphericalAngleBetweenTwoLongLats(double long0, double lat0, double long1, double lat1) {
    return archaversin(haversin(lat1 - lat0) + cos(lat0) * cos(lat1) * haversin(long1 - long0));
}
double haversin(double x) {
    x = sin(x * .5);
    x *= x;
    return x;
}
double archaversin(double x) {
    if      (x >= 1.)   x = M_PI * .5;
    else if (x <= 0.)   x = 0.;
    else                x = asin(sqrt(x)) * 2.;
    return x;
}

The two pitches are the sides of the spherical triangle, and the delta roll is the angle between them. To compute the remaining side, we use the law of haversines. I've also included a solution for a lat-long parameterization of the angles, in case you specify the orientations instead using a yaw-pitch.