I have a body in space, precisely a robots foot, defined by xyz and a roll pitch and yaw. The pitch is along the degree of freedom defined by the pitch joint. I want to rotate the foot using a rotation matrix, and then recover the roll pitch and yaw such that the pitch parameter still represents a rotation as afforded by the pitch joint. Similarly for roll and yaw. There are plenty of tricks for recovering yaw pitch and roll from a rotation matrix, but that doesn't neccesarily guarantee that the parameters corrospond with the axes of rotation on the robot. Does that make sense? Any ideas? Thank you.
Just some thoughts (I am not entirely sure what you are asking for precisely):
1) If you look at the rotation matrix here, you see that you can basically read off the pitch from the lower left entry and knowing the pitch you can read of yaw and roll from the first column and last line respectively.
2) If you have your object in some position and you want to move it wrt to given yaw pitch and roll, then you can just compute the matrix.
3) If you want to move your object from some position to another you can perform the rotation for every degree of freedom sepreately and combine them to find out yaw pitch and roll.
4) If you want to undo a rotation you can compute the inverse of the matrix which will again be a rotation matrix.