ros-industrial / ros2_canopen

CANopen driver framework for ROS2
https://ros-industrial.github.io/ros2_canopen/manual/rolling/
159 stars 71 forks source link

Implement configurable position offsets. #335

Open danzimmerman opened 5 days ago

danzimmerman commented 5 days ago

This PR adds the parameters offset_pos_to_dev and offset_pos_from_dev to bus.yml to allow user-defined offsets in the position feedback and commands. These default to zero if not specified.

One use case is remapping the sense of rotation and angle convention for a joint with an absolute angle encoder whose rotation direction cannot be reversed physically or electronically.

Assuming full-scale encoder count $n{fs}$ and current count $n{enc}$, mapping position feedback from the device, with offset_pos_from_dev = 2*pi, scale_pos_from_dev = -2*pi/n_fs:

$$\large \theta = 2\pi - \frac{2\pi}{n{fs}}n{enc}$$

Mapping commands to the device, with offset_pos_from_dev = n_fs, scale_pos_from_dev = -n_fs/(2*pi):

$$\large n{enc} = n{fs} - \frac{n_{fs}}{2\pi}\theta$$

This could also be useful for absolute position encoders that otherwise cannot be zeroed at a given physical position.