tf2 gimbal lock

edit

Hi!

I need to rotate a point around another point. This is dynamic so there aren't any frames associated that can be published to use the normal tf2 functionality.

What I'm doing is translation to origin, then rotate pitch, then rotate yaw and back to the point it was before.

Everything works pretty well except when both pitch and yaw have the same value of pi/2. Meaning the points are rotated around both y and z in the corresponding angles. But, when the angles are both 90 degrees, only yaw rotation is observed. (I'm assuming it is a Gimbal lock problem).

I have tried both by hand and using the built in functions. Is it really a gimbal lock problem or am I missing something? If yes, how can I avoid the gimbal lock problem?

void learnTf_theHardWay()
{
    tf2::Vector3 toTest_start (0, 0, 0);
    tf2::Vector3 toTest_end (1, 0, 0);

    tf2::Vector3 offset = (toTest_start - toTest_end) / 2;


    tf2::Vector3 origin (0, 0, 0);
    tf2::Vector3 yAxis(0, 1, 0);
    tf2::Vector3 zAxis(0, 0, 1);
    tf2::Quaternion aroundY (yAxis, M_PI/2); 
    tf2::Quaternion aroundZ (zAxis, M_PI/2);
    tf2::Quaternion no_rotation (0, 0, 0, 1);
    tf2::Transform rotation_pitch, rotation_yaw;
    rotation_pitch.setOrigin(origin);
    rotation_pitch.setRotation(aroundY);

    rotation_yaw.setOrigin(origin);
    rotation_yaw.setRotation(aroundZ);

    tf2::Transform translation_to_center;
    translation_to_center.setOrigin(offset);
    translation_to_center.setRotation(no_rotation);


    tf2::Transform translation_from_center;
    translation_from_center.setOrigin(-offset);
    translation_from_center.setRotation(no_rotation);

    tf2::Transform final_transform =  translation_from_center * rotation_yaw * rotation_pitch  * translation_to_center;


    rviz_interface::publish_arrow_path_unreachable(
        octomath::Vector3 (toTest_start.getX(), toTest_start.getY(), toTest_start.getZ()), 
        octomath::Vector3 (toTest_end.getX(), toTest_end.getY(), toTest_end.getZ()), 
        marker_pub, 10);    

    toTest_start = final_transform * toTest_start;
    toTest_end = final_transform * toTest_end;

    rviz_interface::publish_arrow_path_unreachable(
        octomath::Vector3 (toTest_start.getX(), toTest_start.getY(), toTest_start.getZ()), 
        octomath::Vector3 (toTest_end.getX(), toTest_end.getY(), toTest_end.getZ()), 
        marker_pub, 2); 
}

void learnTf_builtInFunctions()
{
    tf2::Vector3 toTest_start (0, 0, 0);
    tf2::Vector3 toTest_end (1, 0, 0);
    tf2::Vector3 origin (0, 0, 0);
    tf2::Quaternion ypr_yaw_pitch ;
    ypr_yaw_pitch.setEuler(M_PI/2, 0, M_PI/2) ; 
    tf2::Transform rotation_yaw_pitch;

    rotation_yaw_pitch.setOrigin(origin);
    rotation_yaw_pitch.setRotation(ypr_yaw_pitch);

    rviz_interface::publish_arrow_path_unreachable(
        octomath::Vector3 (toTest_start.getX(), toTest_start.getY(), toTest_start.getZ()), 
        octomath::Vector3 (toTest_end.getX(), toTest_end.getY(), toTest_end.getZ()), 
        marker_pub, 10);    

    toTest_start = rotation_yaw_pitch * toTest_start;
    toTest_end = rotation_yaw_pitch * toTest_end;

    rviz_interface::publish_arrow_path_unreachable(
        octomath::Vector3 (toTest_start.getX(), toTest_start.getY(), toTest_start.getZ()), 
        octomath::Vector3 (toTest_end.getX(), toTest_end.getY(), toTest_end.getZ()), 
        marker_pub, 2); 
}