Wrap angles in GenericState instances automatically
Description
When two states are added or substracted, angles can become too large, i.e. get out of the required bounds. It is important to keep the angles within the specified bounds, e.g. [0, 2 \pi)
or [-\pi, \pi)
.
Expected behavior
When instances of GenericState
are added or subtracted all angle variables get wrapped automatically.
Definition of Done
-
Adding or subtracting two instances of GenericState
wraps the angles -
Performance is kept reasonable -
Wrapping interval can be picked arbitrarily
Edited by Igor Bogoslavskyi