Rigid Body Constrained Motion Optimization and Control on Lie Groups and Their Tangent Bundles