4X4 to RPY conversion code for CATIA V5 to URDF exporter
I have a project where I will exporting large amounts of CATIA V5 data to DAE and URDF. I am making a URDF and a DAE exporter using CATIA CAA. I don't think CATIA CAA has a conversion utility to go from a 4x4 to x,y,z/r,p,y Where can I find the code that ROS TF uses to convert from a 3x3 rotation matrix to RPY values?