I wanna implement some PD controller for a universal joint. That is, driving the body to a target position. But I can only know the orientations of body instead of the joint angles. How can I get the two angles for universal joint by the body orientation? Thanks! Peng