Rotational error added to pd controller

[dinies]

enhanced pd_controller on error computation of angular quantities when it is dealing with revolute joints.

[costashatz]

Good job. Thanks for the PR. See my inline comments.

[costashatz]

@dinies can you check if the new code is working for you? I did a few tests and it seems OK for me. Once you confirm, I will merge.. Thanks!

[dinies]

It works in my use case, but I only use the controller with prismatic and revolute joints so I could not say anything about the other joint controls (ballJoint, eulerJoint, freeJoint).

Speculation : Is it possible that when we are using the FreeJoint or BallJoint type (since val is of dimension six or three as opposed to a scalar value) to populate the error vector then we could incur dimensionality problems when the error is flattened (.array()) and multiplied and added to _Kd and _Kd*dq ?
Or everything has already been thought to be consistent with the dimensions of the 'generalized dof dimension'.

[costashatz]

Or everything has already been thought to be consistent with the dimensions of the 'generalized dof dimension'.

It does not have to do with the generalized dof dimension, but with the fact that in control we can define which dimensions to control and in arbitrary order. So this means that someone can choose to re-orient the degrees of freedom of the FreeJoint or to control only one of them.

It works in my use case, but I only use the controller with prismatic and revolute joints so I could not say anything about the other joint controls (ballJoint, eulerJoint, freeJoint).

This is enough for now.. Thanks..

[costashatz]

Merging... Thanks a lot @dinies!