Systems and methods disclosed provide for attitude control of a body, the body including a plurality of thrusters and a plurality of gimballed rotors. Operations performed include: receiving a command vector representing a desired torque on the body, generating a Jacobian matrix that, when applied to a combined gimbal rate and thrust command vector, generates the command vector, generating components of a weighted pseudo inverse of the Jacobian matrix and using the components with the command vector in order to generate the combined gimbal rate and thrust command vector. Generating components of the weighted pseudo inverse of the Jacobian matrix can include generating a weighting matrix including a plurality of diagonal elements, the plurality of diagonal elements representing at least the plurality of gimballed rotors and the plurality of thrusters.