fdevinc
commented
3 months ago Hi @passer-by-Wang, thanks for your words and for reaching out! I hope my answer does not come too late! The orientation tracking error in the example is implemented following this paper (eq. 18). It is a pseudometric on the unit quaternions, but a metric on 3D rotations. Unfortunately, the paper does not delve into the effects of weight matrices for assigning different importance to the various components of a rotation; you may want to give this approach a try and see if things work. Alternatively, you could use the function Utils::QuaternionToYawPitchRoll and apply a different weight to the various components of the yaw-pitch-roll (YPR) representation. Beware that this method might give you numerical issues due to the great nonlinearity you would be introducing in the cost function. This would be easily solved by convexifying the Hessian of the corresponding cost terms, but Ungar's optimizer is not mature enough to provide such flexibility and you would have to modify it yourself.
A final option for you would be to have your controller to track three different unit quaternions: one each for yaw, pitch, and roll. Then, you could add a different weight to each tracking term to prioritize one dimension or another. This solution is likely the easiest to implement, so I would try it first. Please let me know if you need further assistance or clarification!
Hello, I am very happy to try ungar library. It is amazing how efficient it is in solving optimization problems and how concise it is in constructing problems. But I have encountered a small problem now. This problem was discovered when I looked at the MPC case of the quadruped robot example. That is, among the weight of each component in the cost function, the weight of the quaternion seems to be a coupled vector, because quaternion is not intuitive for human. On the contrary, euler angles are relatively intuitive. For example, suppose I have a weight vector that costs Euler angles' error. But in order to achieve the same control effect, how do I obtain the corresponding quaternion weight? In other words, how do I adjust the weight of the quaternion to control the change of the Euler angle? Because the change of euler angles is more observable rather than quaternion. I would be grateful if you could give me some help on this issue.