I was wondering why you use the rotational error phi_error = phi_des - phi? phi and phi_des are given in euler XYZ angles and therefore represent a parameterization of the rotation between the two frames (root and world). It's new to me that such an approach works. Why don't you use phi_error = R_des*R^T ? Where R_des and R, are the rotational matrices corresponding to the euler angles?
Subsequently in the PD formulation for the QP you formulated the PD law of the angular acceleration as the sum of the proportional part of the euler error (Kp phi_error ) and the derivative part of the angular rate error in the root frame (Kd (ang_vel_des - ang_vel)). It doesn't add up to me because you mix the euler error, which is a parameterization, with the angular rate error in the root frame.
Another quick follow up question is why you're not considering the inertia for the Quadratic program. Namely: (Af - I omega_dot).
I was wondering why you use the rotational error phi_error = phi_des - phi? phi and phi_des are given in euler XYZ angles and therefore represent a parameterization of the rotation between the two frames (root and world). It's new to me that such an approach works. Why don't you use phi_error = R_des*R^T ? Where R_des and R, are the rotational matrices corresponding to the euler angles?
Subsequently in the PD formulation for the QP you formulated the PD law of the angular acceleration as the sum of the proportional part of the euler error (Kp phi_error ) and the derivative part of the angular rate error in the root frame (Kd (ang_vel_des - ang_vel)). It doesn't add up to me because you mix the euler error, which is a parameterization, with the angular rate error in the root frame.
Another quick follow up question is why you're not considering the inertia for the Quadratic program. Namely: (Af - I omega_dot).
Thanks guys for distributing the codebase. cheers