Closed glopesdev closed 5 months ago
This PR fixes twist angle difference computation to avoid singularities in specific rotation axes. The main difference is computing the twist angle component correctly and ensuring that angle difference is normalized between the range -pi to pi.
Commutator motor feedback sign is updated to ensure negative feedback. Inspired by the swing-twist decomposition breakdown by minorlogic.
This PR fixes twist angle difference computation to avoid singularities in specific rotation axes. The main difference is computing the twist angle component correctly and ensuring that angle difference is normalized between the range -pi to pi.
Commutator motor feedback sign is updated to ensure negative feedback. Inspired by the swing-twist decomposition breakdown by minorlogic.